Systems and methods for using multiple hypotheses in a visual simultaneous localization and mapping system

ABSTRACT

The invention is related to methods and apparatus that use a visual sensor and dead reckoning sensors to process Simultaneous Localization and Mapping (SLAM). These techniques can be used in robot navigation. Advantageously, such visual techniques can be used to autonomously generate and update a map. Unlike with laser rangefinders, the visual techniques are economically practical in a wide range of applications and can be used in relatively dynamic environments, such as environments in which people move. One embodiment further advantageously uses multiple particles to maintain multiple hypotheses with respect to localization and mapping. Further advantageously, one embodiment maintains the particles in a relatively computationally-efficient manner, thereby permitting the SLAM processes to be performed in software using relatively inexpensive microprocessor-based computer systems.

RELATED APPLICATIONS

This application is a continuation application of U.S. application Ser.No. 14/271,260 filed May 6, 2014, now U.S. Pat. No. 9,110,470, whichapplication was a continuation application of U.S. application Ser. No.13/964,979, filed Aug. 12, 2013 (which issued on Sep. 9, 2014 as U.S.Pat. No. 8,830,091), which is a continuation application of U.S.application Ser. No. 13/615,174, filed Sep. 13, 2012, (which issued onAug. 13, 2013 as U.S. Pat. No. 8,508,388), which is a continuationapplication of U.S. application Ser. No. 12/724,312, filed Mar. 15, 2010(which issued on Sep. 25, 2012 as U.S. Pat. No. 8,274,406), which is acontinuation application of U.S. application Ser. No. 11/559,359, filedon Nov. 13, 2006 (which issued on Mar. 16, 2010 as U.S. Pat. No.7,679,532), which is a divisional application of U.S. application Ser.No. 10/739,902, filed Dec. 17, 2003 (which issued on Nov. 14, 2006 asU.S. Pat. No. 7,135,992), which claims the benefit under 35 U.S.C.§119(e) of U.S. Provisional Application No. 60/434,269, filed Dec. 17,2002, and U.S. Provisional Application No. 60/439,049, filed Jan. 9,2003, the entireties of which are hereby incorporated by reference.

This application is also related to commonly-owned application Ser. No.10/739,935, filed on Dec. 17, 2003 (which issued on Aug. 10, 2010 asU.S. Pat. No. 7,774,158); to application Ser. No. 10/739,919, filed onDec. 17, 2003 (which issued on Sep. 18, 2007 as U.S. Pat. No.7,272,467); to commonly-owned application Ser. No. 10/739,937, filed onDec. 17, 2003 (which issued on Mar. 21, 2006 as U.S. Pat. No.7,015,831); to commonly-owned application Ser. No. 10/739,581, filed onDec. 17, 2003 (which issued on Feb. 13, 2007 as U.S. Pat. No.7,177,737); to commonly-owned application Ser. No. 10/739,394, filed onDec. 17, 2003 (which issued on Dec. 5, 2006 as U.S. Pat. No. 7,145,478);and to commonly-owned application Ser. No. 10/739,927, filed on Dec. 17,2003 (which issued on Jan. 9, 2007 as U.S. Pat. No. 7,162,338), theentireties of which are hereby incorporated by reference. Thisapplication is also related to application Ser. No. 11/562,877, filedNov. 22, 2006 (which issued on Aug. 11, 2009 as U.S. Pat. No.7,573,403).

BACKGROUND OF THE INVENTION

Field of the Invention

The invention generally relates to navigation of mobile devices. Inparticular, the invention relates to localization and mapping techniquesthat can be used in vehicles, such as in mobile robots.

Description of the Related Art

Mobile robots are becoming more and more commonplace in society. It willbe understood that these robots can be embodied in a variety of forms,such as in automated vacuum cleaners. A variety of applications can befound for mobile robots, such as, but not limited to, entertainmentapplications, such as toy robots, utility applications in environmentsthat are unfriendly to humans, such as space, deep water, coldtemperature, radiation, chemical exposure, biohazards, etc., dangeroustasks such as defusing of potential explosives, operation in confinedspaces, such as collapsed buildings, the performance of menial tasks,such as cleaning, etc. Conventional robots that are mobile do notinclude automated localization and/or mapping functionality.

Localization techniques refer to processes by which a robot determinesits position with respect to its surroundings. For example, in a “pure”localization system, the robot is provided with a map of itssurroundings. Such “pure” localization systems are disadvantageousbecause generating a map via manual techniques is a relativelydifficult, labor-intensive, and specialized task. Moreover, manyenvironments are not static. For example, the rearranging of furniturein a room can render a preexisting map unusable. As a result, maps inpure localization systems are subject to relatively frequent and costlyupdates such that the map accurately represents its surroundings.

Mapping techniques relate to processes by which a robot builds a map ofits surroundings. A robot that can autonomously build a map of itssurroundings and can localize itself within the map can advantageouslyexhibit a relatively high degree of autonomy. Moreover, such a robot canadvantageously adapt to changes in its surroundings. This process ofbuilding a map and using the generated map is known as SimultaneousLocalization and Mapping (SLAM). It will be understood that while SLAMrelates to the building of a map (mapping) and the use of the map(localizing), a process associated with localization and a processassociated with mapping need not actually be performed simultaneouslyfor a system to perform SLAM. For example, procedures can be performedin a multiplexed fashion. Rather, it is sufficient that a system iscapable of both localization and mapping in order to perform SLAM. Forexample, a SLAM system can use the same data to both localize a vehicle,such as a mobile robot, within a map and also to update the map.

SLAM processes typically use probabilistic techniques, such as BayesianEstimation. Various states of a dynamic system, such as varioushypotheses of a location of a robot and/or a map of robot, can besimultaneously maintained. With probabilistic techniques, a probabilitydensity function represents the distribution of probability over thesevarious states of the system. The probability density function can beapproximated with a finite number of sample points, termed “particles.”

Conventional SLAM techniques exhibit relatively many disadvantages. Forexample, one conventional SLAM technique builds a map using a laserrangefinder. Such laser rangefinder techniques, while accurate, arerelatively unreliable in dynamic environments such as environments wherepeople are walking. In addition, a laser rangefinder is a relativelyexpensive instrument, and can be cost prohibitive for many robotapplications.

One conventional SLAM technique uses an Expectation Maximization (EM)algorithm to generate a map. Disadvantageously, such EM techniques arecomputationally intensive and are difficult to compute in real time.

One conventional technique for localization uses visual images takenfrom a 3-camera array. Disadvantageously, low-production volume multiplecameras cost substantially more than mass-produced single cameras.Moreover, conventional techniques utilizing visual images are notcapable of autonomous mapping.

SUMMARY OF THE INVENTION

Embodiments of the invention advantageously use a visual sensor and deadreckoning sensors to process Simultaneous Localization and Mapping(SLAM). Advantageously, such visual techniques can be used toautonomously generate and update a map. The visual techniques areapplicable in a wide range of applications and can be used in relativelydynamic environments, such as environments in which people move. Oneembodiment further advantageously uses multiple particles to maintainmultiple hypotheses with respect to localization and mapping. Furtheradvantageously, one embodiment maintains the particles in a relativelycomputationally-efficient manner.

One embodiment is a method of localizing a mobile device in amultiple-particle autonomous localization and mapping system, where themethod including: performing autonomous localization and mapping with aplurality of particles, where a particle includes a device pose and amap, where the map includes one or more landmarks; receiving anindication that a landmark has been detected; and updating at least oneparticle from the plurality of particles at least partly in response toreceiving the indication of the detected landmark, wherein updatingfurther includes: selectively categorizing a particle as one of a firsttype of particle and a second type of particle; and calculating anupdated device pose estimate for a particle, wherein the technique usedto calculate the updated device pose estimate is selected according tothe type associated with the particle.

Another embodiment is a circuit for a mobile device that is configuredto localize the mobile device in a multiple-particle autonomouslocalization and mapping system, where the circuit includes: a means forperforming autonomous localization and mapping with a plurality ofparticles, where a particle includes a device pose and a map, where themap includes one or more landmarks; a means for receiving an indicationthat a landmark has been detected; and a means for updating at least oneparticle from the plurality of particles at least partly in response toreceiving the indication of the detected landmark, wherein the means forupdating further comprises: a means for selectively categorizing aparticle as one of a first type of particle and a second type ofparticle; and a means for calculating an updated device pose estimatefor a particle, wherein the technique used to calculate the updateddevice pose estimate is selected according to the type associated withthe particle.

Another embodiment is a computer program embodied in a tangible mediumfor localizing a mobile device in a multiple-particle autonomouslocalization and mapping system, where the computer program includes: amodule with instructions configured to perform autonomous localizationand mapping with a plurality of particles, where a particle includes adevice pose and a map, where the map includes one or more landmarks; amodule with instructions configured to receive an indication that alandmark has been detected; and a module with instructions configured toupdate at least one particle from the plurality of particles at leastpartly in response to receiving the indication of the detected landmark,wherein the module with instructions configured to update furtherincludes: instructions configured to selectively categorize a particleas one of a first type of particle and a second type of particle; andinstructions configured to calculate an updated device pose estimate fora particle, wherein the technique used to calculate the updated devicepose estimate is selected according to the type associated with theparticle.

Another embodiment is a circuit for localizing a mobile device in amultiple-particle autonomous localization and mapping system, where thecircuit includes: a circuit configured to perform autonomouslocalization and mapping with a plurality of particles, where a particleincludes a device pose and a map, where the map includes one or morelandmarks; a circuit configured to receive an indication that a landmarkhas been detected; and a circuit configured to update at least oneparticle from the plurality of particles at least partly in response toreceiving the indication of the detected landmark, wherein the circuitconfigured to update further includes: a circuit configured toselectively categorize a particle as one of a first type of particle anda second type of particle; and a circuit configured to calculate anupdated device pose estimate for a particle, wherein the technique usedto calculate the updated device pose estimate is selected according tothe type associated with the particle.

Another embodiment is a method of localizing a mobile device via amultiple-particle autonomous localization and mapping system, where themethod includes: performing autonomous localization and mapping with aplurality of particles, where a particle includes a device pose estimateand a map, where the map includes one or more landmarks; receiving anindication that a landmark has been recognized and a visually-measuredrelative pose to the landmark has been estimated, where the landmark hasbeen recognized using visual features from a visual sensor coupled tothe mobile device, where the relative pose corresponds to avisually-measured difference in pose between a landmark pose and a posecorresponding to the visual observation; and updating at least one ofthe plurality of particles at least partly in response to receiving theindication of the recognized landmark, wherein updating furthercomprises: using a prior pose estimate and dead reckoning sensor data tocompute a new pose estimate for particles in a selected group based onthe estimated change in pose; and using the landmark pose and thevisually-measured relative pose estimate to compute the new poseestimate for particles not in the selected group.

Another embodiment is a computer program embodied in a tangible mediumfor localizing a mobile device via a multiple-particle autonomouslocalization and mapping system, where the computer program includes: amodule with instructions configured to perform autonomous localizationand mapping with a plurality of particles, where a particle includes adevice pose estimate and a map, where the map includes one or morelandmarks; a module with instructions configured to receive anindication that a landmark has been recognized and a visually-measuredrelative pose to the landmark has been estimated, where the landmark hasbeen recognized using visual features from a visual sensor coupled tothe mobile device, where the relative pose corresponds to avisually-measured difference in pose between a landmark pose and a posecorresponding to the visual observation; and a module with instructionsconfigured to update at least one of the plurality of particles at leastpartly in response to receiving the indication of the recognizedlandmark, wherein the module with instructions configured to updatefurther includes: instructions configured to use a prior pose estimateand dead reckoning sensor data to compute a new pose estimate forparticles in a selected group based on the estimated change in pose; andinstructions configured to use the landmark pose and thevisually-measured relative pose estimate to compute the new poseestimate for particles not in the selected group.

Another embodiment is a circuit for localizing a mobile device via amultiple-particle autonomous localization and mapping system, where thecircuit includes: a circuit configured to perform autonomouslocalization and mapping with a plurality of particles, where a particleincludes a device pose estimate and a map, where the map includes one ormore landmarks; a circuit configured to receive an indication that alandmark has been recognized and a visually-measured relative pose tothe landmark has been estimated, where the landmark has been recognizedusing visual features from a visual sensor coupled to the mobile device,where the relative pose corresponds to a visually-measured difference inpose between a landmark pose and a pose corresponding to the visualobservation; and a circuit configured to update at least one of theplurality of particles at least partly in response to receiving theindication of the recognized landmark, further includes: a circuitconfigured to use a prior pose estimate and dead reckoning sensor datato compute a new pose estimate for particles in a selected group basedon the estimated change in pose; and a circuit configured to use thelandmark pose and the visually-measured relative pose estimate tocompute the new pose estimate for particles not in the selected group.

BRIEF DESCRIPTION OF THE DRAWINGS

These and other features of the invention will now be described withreference to the drawings summarized below. These drawings (not toscale) and the associated descriptions are provided to illustratepreferred embodiments of the invention and are not intended to limit thescope of the invention.

FIG. 1 illustrates an example of a robot.

FIG. 2 is a top view of a room with obstacles illustrating an actualpath taken by a robot, a first path estimated through dead reckoningmeasurements alone, a second path estimated through dead reckoningmeasurements alone, and a path estimated through visual simultaneouslocalization and mapping (VSLAM).

FIG. 3A illustrates the use of multiple cameras to determine a 3-Dposition of a visual landmark.

FIGS. 3B and 3C illustrate the use of a single camera to determine a 3-Dposition of a visual landmark.

FIG. 4 illustrates a pose of a robot in a global reference frame.

FIG. 5A illustrates a robot reference frame in the context of creating arecord in a landmark database.

FIG. 5B illustrates a landmark reference frame and a robot referenceframe in the context of revisiting a landmark.

FIG. 5C illustrates the convention used to describe a Δx and a Δycalculation.

FIG. 5D illustrates the convention used to describe a Δθ calculation.

FIG. 6 illustrates one embodiment of a system architecture for a VSLAMsystem.

FIG. 7A illustrates multiple particles in a multiple-hypothesis VSLAMsystem.

FIG. 7B illustrates one example of data associated with a hypothesis orparticle.

FIG. 8 illustrates one example of a data structure for a relationaldatabase that can be used with an embodiment of the invention tomaintain data for a visual front end and for SLAM.

FIG. 9 is a flowchart that generally illustrates a process useful in avisual front end for visual processing.

FIG. 10 is a flowchart that generally illustrates a process useful in avisual front end for creating a new landmark.

FIG. 11 is a flowchart that generally illustrates a process useful in aSLAM module for creating a new landmark.

FIG. 12 is a flowchart that generally illustrates a process useful in apre-filter for removing outliers.

FIG. 13 consists of FIGS. 13A, 13B, and 13C and is a flowchart thatgenerally illustrates a process useful in a SLAM module whenre-encountering a landmark.

FIG. 14 illustrates one embodiment of a system architecture for visuallocalization.

FIG. 15 illustrates an embodiment of a pre-filter for filteringoutliers.

GLOSSARY OF TERMS

pose: the position and orientation, such as the position and orientationof a robot, in some reference frame.

robot pose (also known as global robot pose): the position andorientation of a robot in a global reference frame. In a configurationwhere a robot travels in two dimensions, such as along the surface of afloor, the robot pose can be specified by a two-dimensional position(x,y) and a heading (θ).

relative robot pose: the position and orientation of a robot withrespect to another reference frame, such as a landmark reference frame.

global reference frame: a reference frame that is fixed to theenvironment.

landmark reference frame: the reference frame in which a landmark's 3-Dstructure is defined.

3-D structure: the 3-D coordinates of a set of 3-D features.

landmark: a landmark comprises a collection of 3-dimensional (3-D)features and a unique identifier.

3-D feature: an observable location, such as, for example, a portion ofan object, with an associated 3-D coordinate in a reference frame andone or more associated 2-D features observable when viewing thelocation. It will be understood that a 3-D feature can be observed fromone or more perspectives with varying 2-D features.

2-D feature: a position in an image and a descriptor that relates to thepixel at the position or the pixels in some neighborhood around thatposition.

physical landmark: a collection of visually-identifiable 3-D features inthe environment.

landmark pose: the pose of the landmark reference frame in the globalreference frame.

camera pose: a relative pose in the landmark reference frame based onthe location of the visual sensor, which can be, for example, a camera.

DETAILED DESCRIPTION OF PREFERRED EMBODIMENTS

Although this invention will be described in terms of certain preferredembodiments, other embodiments that are apparent to those of ordinaryskill in the art, including embodiments that do not provide all of thebenefits and features set forth herein, are also within the scope ofthis invention.

Embodiments of the invention advantageously use one or more visualsensors and one or more dead reckoning sensors to process SimultaneousLocalization and Mapping (SLAM). The combination of SLAM with visualsensors will hereafter be referred to as VSLAM. Advantageously, suchvisual techniques can be used by a vehicle, such as a mobile robot, toautonomously generate and update a map. In one embodiment, VSLAM isadvantageously used by a portion of a vehicle, such as by an “arm” of avehicle. In contrast to localization and mapping techniques that uselaser rangefinders or other range-based devices or sensors, the visualtechniques are economically practical in a wide range of applicationsand can be used in relatively dynamic environments, such as environmentsin which people move. One embodiment further advantageously usesmultiple particles to maintain multiple hypotheses with respect tolocalization and mapping. Further advantageously, one embodimentmaintains the particles in a relatively computationally-efficientmanner, thereby permitting the SLAM processes to be performed insoftware using relatively inexpensive microprocessor-based computersystems.

It will be understood by one of ordinary skill in the art that mobilerobots can be embodied in a variety of forms. In these variety of forms,a robot may be referred to by a different name, such as by a functionthat is performed by the robot. For example, a robot may be referred toas an automated sweeper or as an automated vacuum cleaner. In oneembodiment, a mobile robot corresponds to a self-propelled object thatcan navigate in an autonomous or semi-autonomous manner. Examples ofautonomous or semi-autonomous mobile robots include, but are not limitedto, mobile robots for use in automated floor cleaners, humanoid robots,robots for experimentation and lab use, robots for delivering supplies,robots for exploring confined or inaccessible spaces, robots forentertainment or play, and the like.

The VSLAM techniques disclosed herein can advantageously be applied toautonomous robots and to non-autonomous robots. For example, the VSLAMtechniques can be used with a manually-driven vehicle, such as aremotely-controlled vehicle for bomb detection. For example, the VSLAMtechniques can be advantageously used in a remote-control application toassist an operator to navigate around an environment. In one embodiment,a vehicle can include various operational modes, such as a mode formanual control of the vehicle and another mode for an autonomous controlof the vehicle. For example, the vehicle can be manually-driven duringan initial mapping stage, and then later, the vehicle can be configuredfor autonomous control. In another embodiment, the VSLAM techniques canbe used by a scout to create a map of the region. The scout cancorrespond to, for example, a person or another animal, such as a dog ora rat. The VSLAM used by the scout can be coupled to a video cameracarried by the scout to observe the environment and to a dead reckoningdevice, such as an odometer, a pedometer, a GPS sensor, an inertialsensor, and the like, to measure displacement. The map generated by thescout can be stored and later used again by the scout or by anotherentity, such as by an autonomous robot. It will be understood thatbetween the generation of the map by the scout and the use of the map byanother entity, there can be additional processing to accommodatedifferences in visual sensors, differences in the installed height ofthe visual sensor, and the like.

Robots can be specified in a variety of configurations. A robotconfiguration typically includes at least one dead reckoning sensor andat least one video sensor. Another name for dead reckoning is “ded”reckoning or deduced reckoning. An example of a dead reckoning sensor isa wheel odometer, where a sensor, such as an optical wheel encoder,measures the rotation of a wheel. The rotation of wheels can indicatedistance traveled, and a difference in the rotation of wheels canindicate changes in heading. With dead reckoning, the robot can computecourse and distance traveled from a previous position and orientation(pose) and use this information to estimate a current position andorientation (pose). While relatively accurate over relatively shortdistances, dead reckoning sensing is prone to drift over time. It willbe understood that the information provided by a dead reckoning sensorcan correspond to either distance, to velocity, or to acceleration andcan be converted as applicable. Other forms of dead reckoning caninclude a pedometer (for walking robots), measurements from an inertialmeasurement unit, optical sensors such as those used in optical mousedevices, and the like. Disadvantageously, drift errors can accumulate indead reckoning measurements. With respect to a wheel odometer, examplesof sources of drift include calibration errors, wheel slippage, and thelike. These sources of drift can affect both the distance computationsand the heading computations.

An example of a visual sensor is a digital camera. Embodiments of theinvention advantageously use a visual sensor to recognize landmarks on avisual basis. These observations of visual landmarks can advantageouslyprovide a global indication of position and can be used to correct fordrift in the dead reckoning sensors. In contrast to simultaneouslocalization and mapping (SLAM) techniques that use a laser rangefinder,embodiments of the invention can use data from visual sensors and fromdead reckoning sensors to provide simultaneous localization and mapping(SLAM) with advantageously little or no additional cost.

Exemplary Robot with VSLAM

FIG. 1 illustrates an example of a mobile robot 100 in which a VSLAMsystem can be incorporated. The illustrated robot 100 includes a visualsensor 104, which is used to visually recognize landmarks such that aSLAM module can determine global position. A broad variety of visualsensors can be used for the visual sensor 104. For example, the visualsensor 104 can correspond to a digital camera with a CCD imager, a CMOSimager, an infrared imager, and the like. The visual sensor 104 caninclude normal lenses or special lenses, such as wide-angle lenses,fish-eye lenses, omni-directional lenses, and the like. Further, thelens can include reflective surfaces, such as planar, parabolic, orconical mirrors, which can be used to provide a relatively large fieldof view or multiple viewpoints. In another example, the visual sensor104 can correspond to a single camera or to multiple cameras. In oneembodiment, the VSLAM system is advantageously configured to operatewith a single camera, which advantageously reduces cost when compared tomultiple cameras.

The motors 110, 112 of the illustrated robot 100 are coupled to wheels114, 116 to provide locomotion for the robot 100. It will be understoodby one of ordinary skill in the art that instead of or in addition towheels, other embodiments of the robot can use legs, tracks, rollers,propellers, and the like, to move around. In the illustrated embodiment,information regarding the rotation of the wheels, also known asodometry, is provided as an input to a control 108. Image data 106 fromthe visual sensor 104 is also provided as an input to the control 108for the robot 100. In one embodiment, the VSLAM system is embodiedwithin the control 108. In the illustrated embodiment, the control 108is coupled to motors 110, 112 to control the movement of the robot 100.For clarity, a power source for the robot 100, such as a battery, is notshown in FIG. 1.

In response to the image data 106, the control 108 can provide controlsignals to the motors 110, 112 that control the movement of the robot100. For example, the control 108 can provide control signals toinstruct the robot to move forward, to stop, to move backward, to turn,to rotate about a vertical axis, and the like. When the robot rotatesaround a vertical axis, such as the exemplary vertical axis 118 shown inFIG. 1, this rotation is referred to as “yaw.” Convention used herein todescribe the pose, that is, the orientation and the position of therobot, will be described in greater detail later in connection with FIG.4.

The control 108 can include hardware, such as microprocessors, memory,etc., can include firmware, can include software, can include networkcommunication equipment, and the like. In one embodiment, the control108 uses dedicated hardware, such as single-board computers, applicationspecific integrated circuits (ASICs), field programmable gate arrays(FPGAs), and the like. Examples of system architectures for a VSLAMsystem will be described in greater detail later in connection withFIGS. 6, 14, and 15.

In one embodiment, the control 108 is implemented by interfacing to ageneral-purpose computer, such as to a laptop computer, and by softwareexecuting in the computer. In one example, a laptop computer with anIntel® Pentium® 4 processor with a 2.4 GHz clock speed can processlandmark generation processes in about 1 second and can process visualmeasurements in about half a second. It will be understood that theprocessing time can depend on parameters such as image resolution, framerates, bus speeds, and the like. The software can include instructionsthat are embodied in a tangible medium, such as a hard disk or anoptical disk. Data processing for the robot 100 can be performedentirely within the robot 100 such that the robot 100 is autonomous, orthe data processing can be performed partially outside the robot 100.For example, the control 108 can be configured to relay data to anothercomputer, via a network such as a wireless network, where a portion ofthe data processing takes place. An example of a data structure for arelational database for use in a VSLAM system will be described later inconnection with FIG. 8. Examples of data processing will be described ingreater detail later in connection with FIGS. 9-13.

An Example of Experimental Results

FIG. 2 is a top view of an apartment with objects, such as wallsfurniture, and the like. FIG. 2 illustrates an actual path 202 taken bya robot, a first path 204 estimated through dead reckoning measurementsalone, a second path 206 estimated through dead reckoning measurementsalone, and a path estimated through VSLAM 208.

In one embodiment, the global position of the robot is represented byCartesian (x-y) coordinates as shown in FIG. 2. It will be understoodthat other coordinate systems, such as polar coordinates, can also beused. With respect to FIG. 2, a horizontal axis 210 corresponds to thex-axis. A vertical axis 212 corresponds to the y-axis. The coordinatesystem used herein will be described in greater detail later inconnection with FIG. 4.

In one embodiment, the N-dimensional zero vector (0,0,0) for (x,y,θ) forthe system is set when the robot is initialized or is reset. Unless thezero vector is reset by, for example, a deliberate reset, a systemreset, re-initialization process, and the like, the zero vector staysfixed for the global reference frame. By contrast, a relative referenceframe with a moving zero vector relative to the global reference framewill be described later in connection with FIGS. 5A and 5B. In addition,it will be understood that the N-dimensional zero vector can furtherinclude other dimensions, such as height z, roll, and pitch, asapplicable. Returning now to the example shown in FIG. 2, a zero vector214 is located at approximately the entrance to the apartment, but itwill be understood that the location of the zero vector 214 can belocated virtually anywhere. In addition, it should be noted that thesystem can be configured to set the zero vector to locations other thanwhere the robot is initialized or is reset, such as, for example, alocation where a landmark is first observed.

As a robot with VSLAM travels in its environment, such as the apartmentshown, the robot can observe physical landmarks. As will be explained ingreater detail later, these physical landmarks can be related tolandmarks created and stored in a database. Advantageously, the VSLAMtechniques do not require artificial navigational beacons to be placedin the environment. Rather, VSLAM techniques can conveniently be used inunaltered and unmodified environments. However, it will be understoodthat should artificial navigational beacons be present in anenvironment, the VSLAM techniques can utilize features from the beaconsand/or the surrounding environment as landmarks. For example, in alandmarks database, where a landmark can correspond to a collection of3-D features and the corresponding 2-D features from which the 3-Dfeatures are computed. It should also be noted that a physical landmarkcan correspond to one or more physical objects, such as, for example, anobject mounted to a wall and a portion of the wall. These physicallandmarks are used to estimate global position such that drift in deadreckoning measurements can later be corrected or compensated. It shouldbe noted that a physical landmark will typically be arranged in aparticular location and orientation in the global reference frame, andthat the observing robot will be at a different location andorientation. In the illustrated embodiment, the locations of thefeatures of the physical landmark are referenced relative to thelandmark reference frame. Then, the pose of the landmark itself isreferenced to the global reference frame.

The estimated poses of selected landmarks are illustrated in FIG. 2. InFIG. 2, examples of global position (x,y) for the robot pose when therobot perceives a physical landmark are indicated by circles. It will beunderstood that while the VSLAM system is generally described in thecontext of a robot, the VSLAM system can also be used in a variety ofdevices such that the robot pose can also correspond to a device pose.The orientation (θ) of the robot as it observes the physical landmarkand creates the landmark in the database is indicated with an arrow. Inone embodiment, the initial estimate of the pose of the “landmark” thatis referenced in the global reference frame corresponds to the pose ofthe robot when creating the landmark. When a new physical landmark isobserved and a landmark is created, the set of 3-D features and theircorresponding 2-D features that visually identify the landmark arestored. In one example, the 2-D features correspond to SIFT features.The concept of SIFT has been extensively described in the literature.See David G. Lowe, Local Feature View Clustering for 3D ObjectRecognition, Proceedings of the IEEE Conference on Computer Vision andPattern Recognition, Kauai, Hi. (December 2001).

As illustrated in FIG. 2, the path estimated through VSLAM 208 isadvantageously much more accurate than the second path 206 estimatedthrough dead reckoning measurements alone. Over relatively shortdistances, dead reckoning measurements, such as those obtained fromodometry readings, can be quite accurate. For example, the first path204 taken after a single pass estimated through dead reckoningmeasurements alone is relatively close to the actual path 202. However,due to calibration errors, wheel slippage, and the like, these deadreckoning measurements can drift or accumulate errors over distanceand/or time such that a position calculated after a relatively longperiod of time can vary significantly from a position that was initiallycalculated even when the errors in the dead reckoning measurements arerelatively small. For example, over an extended period of time, therobot can make relatively many traversals over an environment, therebyaccumulating errors in drift. In the example illustrated in FIG. 2, thesecond path 206 estimated through dead reckoning measurements alone wascalculated after about 6 passes around the apartment. As illustrated byFIG. 2, the disparity between the actual path 202 and the second path206 estimated through dead reckoning measurements alone can bepronounced.

Advantageously, the VSLAM techniques can wholly or partially compensatefor the drift in the dead reckoning measurements such that even after arobot has traveled over relatively large distances, the global positionof the robot can be maintained with relatively high accuracy. In oneembodiment, the VSLAM techniques maintain the accuracy of the globalrobot pose estimate to exceed the accuracy of the visual measurementseven after the robot has traversed relatively long distances. In theillustrated example, the visual sensor used was a relatively simple andinexpensive color camera with a resolution of 640×480, and the accuracyof the visual measurements was maintained to about 10 centimeters (cm).It will be understood that other visual sensors, such as grayscalecameras and infrared cameras, can also be used. In FIG. 2, the pathestimated through VSLAM 208 was computed under the same conditions asthe second path 206 estimated through dead reckoning measurements alone.As illustrated by FIG. 2, the path estimated through VSLAM 208 isrelatively close to the actual path 202 and is advantageously relativelymore accurate than the second path 206 estimated through dead reckoningmeasurements alone.

Examples of Different Perspectives for Visual Localization

FIG. 3A illustrates the use of multiple cameras or multiple visualsensors to determine a 3-D position of a physical landmark for VSLAM. Inone embodiment, a robot 302 is equipped with multiple visual sensorsthat are spaced a known distance apart to provide a stereoscopic view,such as a binocular view or a trinoculor view, of a physical landmark306. An example of a trinoculor camera is the Digiclops™ cameraavailable from Point Grey Research Inc., of Vancouver, British Columbia,Canada.

Advantageously, camera systems that provide a stereoscopic view, such asbinocular or trinoculor camera systems, can be used to identify 3-Dfeatures of a landmark and to estimate displacements to the 3-D featuresin a relatively fast and efficient manner. Disadvantageously, suchcameras are produced in relatively low volumes and can be relativelyexpensive relative to single visual sensor cameras due to the extracomponents and to the relative lack of economies of scale.

FIGS. 3B and 3C illustrate the use of a single camera to estimate the3-D structure of a landmark 308. The use of a single camera in theprocessing of VSLAM advantageously permits relatively common andinexpensive visual sensors to be used for VSLAM visual processing.Moreover, many configurations of robots are already configured with asingle camera, which can be used with the VSLAM processing such that noadditional camera is needed.

To obtain multiple perspective views from images taken from a singlecamera, the images are taken from different locations, such as, forexample, from 3 different positions. For example, FIGS. 3B and 3Cillustrate how a robot 304 moves from a first position as shown in FIG.3B to a second position as shown in FIG. 3C. In both FIGS. 3B and 3C,the robot 304 observes the same physical landmark 308. In oneembodiment, the robot 304 monitors the distance traveled by odometry oranother dead reckoning source, and selects at least 2 images, such as,for example, 2 or 3 images, taken at about 10 centimeters (cm) apart forthe perspective views. It will be understood that an appropriate amountof distance traveled between selected images can vary in a broad rangeand can vary depending on the environment in which the robot 304operates. In one embodiment, the robot 304 travels a minimumpredetermined distance between images used for the perspective views.This minimum predetermined distance can be adaptively determined atleast partly in response to the operating environment of the robot 304.For example, the minimum predetermined distance can correspond to afixed percentage, such as 5%, of an average displacement to observedfeatures. Other applicable percentages will be readily determined by oneof ordinary skill in the art. These perspective views can be used todetermine the positions of 3-D features of new landmarks. As will beexplained in greater detail later, in one embodiment, such perspectiveviews are not used when a landmark is revisited.

Illustrations of Robot Pose and Global Reference Frame

The “pose” of a robot includes the position of the robot and theorientation or heading of the robot within the global reference frame.FIG. 4 illustrates a position (x,y) of the pose of a robot 402 in aglobal reference frame. It will be understood that the position does nothave to correspond precisely to the center of the robot 402, as therobot 402 occupies an area of space larger than a point. A zero vector406 for the global reference frame can correspond to one of a variety oflocations within an environment. In the illustrated examples, the zerovector 406 for the global reference frame is set at the location wherethe robot 402 was initialized. In one embodiment, the x-direction forthis global reference frame corresponds to an axis directly in front ofthe robot when the robot was initialized. For example, in theillustrated embodiment, the x-axis corresponds to the path traveled bythe robot when the robot travels directly in the forward direction uponinitialization at the zero vector 406. In the illustrated embodiment,the y-direction corresponds to an axis orthogonal to the x-axis, withthe positive y-axis increasing to the left of the x-axis when viewedfrom above. It will be understood that the zero vector for referenceframe and the alignment of the x-axis and the y-axis with respect to thezero vector can depend on where the robot is located and the orientationof the robot at the time the zero vector is set, such as, for example,at the time of initialization.

In addition to displacement along the x-axis direction or the y-axisdirection, a typical mobile robot can also rotate or “yaw” about avertical axis. It will be understood that robots do not have to directlyrotate or yaw to change orientation. For example, the act of turningaround a corner also changes the orientation or heading of the robot.

FIG. 4 also illustrates an orientation of the pose of a robot in aglobal reference frame. In the illustrated embodiment, the orientation θfor the robot is referenced to the initial heading that the robot hadwhen the zero vector was set. Advantageously, the location of the robotand the initial orientation for the robot is arbitrary, therebysimplifying setup of the robot for the user.

In one embodiment, the global reference frame described in FIG. 4 isused by the SLAM portion of the VSLAM, such as a SLAM module 604 whichwill be described in greater detail later in connection with FIG. 6. Bycontrast, a relative reference frame is used for the visual front-endportion of VSLAM, such as by a Visual Front End 602 of FIG. 6. Therelative reference frame will also be described in further detail laterin connection with FIGS. 5A and 5B.

Illustrations of Visual Measurements

FIGS. 5A and 5B (not to scale) illustrate a robot 502 and acorresponding robot reference frame 520. In the illustrated embodiment,the robot reference frame 520 is used by the visual localization portionof a VSLAM system, such as the Visual Front End 602 of FIG. 6. The zerovector for the robot reference frame 520 moves with the robot 502. Assuch, the robot reference frame 520 is a relative reference frame, asopposed to a global reference frame that has a globally-fixed zerovector. For example, the zero vector for the robot reference frame 520can be located approximately at the camera of the robot 502 and isillustrated in FIG. 5A by a pose “A” 522 and in FIG. 5B by a pose “B”524.

As the robot 502 travels in its environment, the robot 502 detects newphysical landmarks and revisits previously detected or “old” physicallandmarks. FIG. 5A illustrates the robot reference frame 520 in thecontext of “creating” or recognizing a new landmark, i.e., creating anentry in a database for a freshly observed landmark. A process in avisual front end or visual localization process for recognizing a newlandmark will be described in greater detail later in connection withFIG. 10. FIG. 5B illustrates the robot reference frame 520 in thecontext of revisiting a previously observed and recorded landmark. Therobot reference frame 520 moves with the robot 502 such that the pose“A” 522 corresponding to the pose of the robot, with respect to theglobal reference frame, at the time when the landmark was created, andthe pose “B” 524 corresponding to the pose of the robot, with respect tothe global reference frame, at the time when the landmark was revisitedcan be different as illustrated in FIG. 5B.

Returning now to FIG. 5A, in the illustrated embodiment, a physicallandmark 504 is identified by its 3-D features. In one embodiment, 3-Dfeatures are extracted by triangulating 2-dimensional (2-D) features bysolving the structure and motion problem using the trifocal tensormethod. In one embodiment, the 2-D features are SIFT features. Adiscussion of SIFT features can be found in Lowe, id. See OlivierFaugeras and Quang-Tuan Luong, The Geometry of Multiple Images, MITPress (2001) for a discussion of the trifocal tensor method. It will beunderstood that the physical landmark 504 can be characterized byrelatively many 3-D features and that the physical landmark 504 cancorrespond to one or more physical objects or to a portion of physicalobject. For clarity, the physical landmark 504 illustrated in FIG. 5A isdrawn with 3 3-D features: a first feature 506, a second feature 508,and a third feature 510. When the robot 502 observes a new physicallandmark, the visual front end determines the displacements or positionsfrom the robot 502 to the respective features. When a landmark iscreated, the robot 502 can reference displacements to visual featuresusing the current position of the robot reference frame 520 as aninitial estimate of a landmark reference frame. For example, in theexample illustrated in FIG. 5A, arrows r₁, r₂, and r₃ represent3-dimensional displacements, such as displacements in x, y, and zdimensions between the robot 502 and the first feature 506, the secondfeature 508, and the third feature 510, respectively. It should be notedthat these x, y, and z displacements are relative to the robot referenceframe of the robot 502 and not to the global reference frame. In oneembodiment, the x, y, and z displacements correspond to relativedisplacements in the fore-aft dimension, in the left-right dimension,and in the up-down dimension, respectively. In addition, the 2-D imagecoordinates or locations for the 3-D features are also stored. Forexample, where the visual sensor corresponds to a 640×480 color camera,the 2-D image coordinates correspond to one or more pixel locations thatcorrespond to the 3-D features. It will be understood that 3-D featureswill typically occupy more than merely a single point in space.

In one embodiment, where the robot 502 moves as the images are taken forthe perspective views for the computation of the displacements r₁, r₂,and r₃, the displacements from the robot 502 to the features arereferenced to the first image in a three-image set. However, it will beappreciated that any identifiable reference frame can be used as thereference. For example, the other images in the image set can also beused as the reference, so long as the image used as the reference isconsistently selected. It should also be noted that an identifiablereference frame that does not correspond to any particular image canalso be used. For example, in the illustrated embodiment, the pose ofthe robot corresponding to the first image in the three-image set isused as the local reference frame for the particular landmark, i.e., thelandmark reference frame.

In one embodiment, when a new physical landmark is encountered, theVisual Front End 602 stores (i) the 3-D coordinates of the 3-D featuresin the local reference frame for the landmark in a database, such as alandmark database 606 of FIG. 6 and (ii) the 2-D features for a selectedimage, such as the 2-D features of the first image, corresponding to the3-D features. An example of a data structure for the landmark database606 will be described later in connection with FIG. 8. In oneembodiment, when the new physical landmark is encountered and processedby the Visual Front End 602, the SLAM module 604 correspondingly“creates” a landmark by storing an initial estimate of the landmarkpose, such as the global robot pose when the landmark was created,computed from the change in pose as provided by the dead reckoning datafor each particle from the last pose of the robot for the correspondingparticle. An example of a process that can be performed by the SLAMmodule 604 when creating a new landmark will be described in greaterdetail later in connection with FIG. 11.

FIG. 5B illustrates an example of the robot 502 revisiting the physicallandmark 504 earlier observed, termed “new view.” In FIG. 5B, the robot502 is displaced from the original pose “A,” which corresponds to the“landmark reference frame,” to a new pose “B.” Correspondingly, therobot reference frame also moves with the robot 502. The robot 502 againobserves the first feature 506, the second feature 508, and the thirdfeature 510. It will be understood that as the robot 502 moves about,some of the features of a physical landmark may not be observable in alllocations. The Visual Front End 602 of the robot 502 computes therelative pose, i.e., the difference between new pose “B” and pose “A,”as illustrated in FIGS. 5C and 5D and provides one or more relativeposes to one or more identified landmarks as an input to the SLAM module604 or to the Pre-Filter module 622. In one embodiment, the Visual FrontEnd 602 computes the relative pose of the robot with respect to thestored landmark reference frame illustrated as “A” by finding therelative pose of the robot that minimizes the projection error from the3-D features onto the 2-D coordinates of the first feature 506, thesecond feature 508, and the third feature 510.

Dashed lines between the robot 502 and the features 506, 508, 510represent the projection of the features 506, 508, 510 onto an imageplane, which is illustrated by a line 530 in the top view of FIG. 5B. Itwill be understood that the image plane for a visual sensor, such as acamera, will typically be approximately perpendicular to the focal axisof the camera. It will also be understood that the line 530approximately represents the field of view of the camera for theprojection of the points and does not indicate any particular distancefrom the camera.

Given the correspondence between the 2-D features in the new view andthe 3-D features of the landmark, the Visual Front End 602 can estimatethe relative pose by, for example, minimizing projection error. Therelative pose reveals a change in pose from (i) the pose when thelandmark was created and stored in the database to (ii) the pose whenthe physical landmark was re-observed. It will be understood that such arelative pose can be represented in a variety of coordinate forms. Forexample, the translational component of the relative pose along thefloor can be represented by Cartesian coordinates (x,y). However, itwill also be understood that polar coordinates (ρ,φ) can also be used.FIG. 5C and FIG. 5D graphically illustrate the relative pose also knownas “camera pose” components of Δx, Δy, and Δθ. While the term “camerapose” includes the word “camera,” it will be understood that visualsensors other than cameras can also be used. The relative pose can alsoinclude a change in vertical dimension, roll, and pitch, which can bethe result of uneven floor surfaces, robot and/or camera movement inthese dimensions, misidentified landmarks, changes in the physicallandmarks in the environment, and the like. In one embodiment, theseadditional dimensions are advantageously used to test the validity ofthe identified landmark, as will be described later in greater detail inconnection with the Pre-Filtering process of FIG. 12. In one embodiment,the Cartesian-coordinate relative pose is used between a visualfront-end and a SLAM module when re-encountering landmarks, and apolar-coordinate “delta pose” is used in the SLAM module when computingchange in pose between points measured by dead reckoning data.

In one embodiment, the pose of the robot according to dead reckoningsensor data as the robot travels in its environment is stored with acorresponding timestamp in a matrix, such as in a Dead. Reckoning DataMatrix 812, which will be described later in connection with FIG. 8.Retrieval of poses according to two points in time permits change inheading, direction traveled, and distance traveled to be computedbetween the two points in time.

Example of a System Architecture for VSLAM

FIG. 6 illustrates one embodiment of a system architecture for a VSLAMsystem 600. It will be understood that the VSLAM system 600 can beimplemented in a variety of ways, such as by dedicated hardware, bysoftware executed by a microprocessor, or by a combination of bothdedicated hardware and software.

Inputs to the VSLAM system 600 include raw pose data 610 from one ormore dead reckoning sensors 614 and also include visual data 612 fromone or more cameras or other visual sensors 616. It will be understoodthat a dead reckoning sensor 614, such as an optical wheel encoder, cancommunicate with the VSLAM system 600 via a dead reckoning interface618, such as via a driver or via a hardware abstraction layer. The rawpose data 610 can correspond to distance traveled, to velocity, toacceleration, and the like, and can depend on the type of dead reckoningsensor used. Outputs from the VSLAM system 600 can include one or moreposes and maps 620.

The raw pose data 610 and the visual data 612 are provided as inputs tothe Visual Front End 602. The Visual Front End 602 can perform a varietyof functions, such as identify landmarks, identify 3-D features forlandmarks, calculate delta pose, and the like. Examples of processesthat can be performed by the Visual Front End 602 will be described ingreater detail later in connection with FIGS. 9 and 10. A systemarchitecture for the Visual Front End 602 will be described in greaterdetail later in connection with FIG. 14.

The Visual Front End 602 can use the raw pose data 610 to determine theapproximate distance traveled between the images in the visual data 612,which are then used in computations to measure the displacements to thefeatures. When new physical landmarks are recognized, correspondingrecords or entries can be added to the landmark database 606. A sampledata structure for the landmark database 606 will be described later inconnection with FIG. 8. Newly recognized landmarks can also be indicatedto the SLAM module 604. For example, a “new landmark” flag can beactivated, and a “new landmark” identifier or tag can be provided to theSLAM module such that the appropriate records in a SLAM database 608 andthe landmark database 606 can be matched. When previously recognizedlandmarks are encountered, the Visual Front End 602 can provide the SLAMmodule 604 or an optional Pre-Filter module 622 with one or moreidentifiers or tags to indicate the one or more landmarks encountered,relative pose information, such as relative pose information (Δx, Δy,and Δθ), and data reliability measures as applicable.

The optional Pre-Filter module 622 analyzes the data reliabilitymeasures provided by the Visual Front End 602. The data reliabilitymeasures can be used as an indication of the reliability of theidentification of the physical landmark by the Visual Front End 602. Forexample, the Pre-Filter module 622 can advantageously be used toidentify a landmark measurement identified by the Visual Front End 602,which may have been inaccurately identified and can correspond to anoutlier with respect to other landmarks in a map. In one embodiment,when the Pre-Filter module 622 identifies a potentially inaccuratevisual measurement, the Pre-Filter module 622 does not pass theidentified visual landmark data onto the SLAM module 604 such that theVSLAM system 600 effectively ignores the potentially inaccurate landmarkmeasurement. Pre-filtering of data to the SLAM module 604 canadvantageously enhance the robustness and accuracy of one or more poses(position and orientation) and maps 620 estimated by the SLAM module604.

A process that can be performed by the Pre-Filter module 622 will bedescribed in greater detail later in connection with FIG. 12. A systemarchitecture of the Pre-Filter module 622 will be described in greaterdetail later in connection with FIG. 15.

The SLAM module 604 maintains one or more poses and maps 620. In oneembodiment, the SLAM module 604 maintains multiple particles orhypotheses, and each particle is associated with a pose and a map. FIGS.7A and 7B illustrate an example of relationships between particles,poses, and maps.

The SLAM module 604 receives the raw pose data 610 from the deadreckoning interface 618. It will be understood that the nature of theraw pose data 610 can vary according to the type of dead reckoningsensor 614 and the type of output specified by the dead reckoninginterface 618. Examples of the raw pose data 610 can include distancemeasurements, velocity measurements, and acceleration measurements. Thedead reckoning data is used by the SLAM module 604 to estimate courseand distance traveled from a prior pose. It will be understood thatwhere multiple hypotheses are used by the SLAM module 604, that the deadreckoning data is used to estimate course and distance traveled fromrelatively many prior poses.

Other inputs to the SLAM module 604 include visual localization datafrom the Visual Front End 602 and/or the optional Pre-Filter module 622.As a robot with VSLAM travels in an environment, the robot observesvisual landmarks. When a new visual landmark is encountered, the SLAMmodule 604 can store the robot's global reference frame location for theparticles in the SLAM database 608. For example, the robot's pose can beestimated from a previous location and the course and distance traveledfrom a last known pose.

When a previously created landmark is observed, the SLAM module 604 isprovided with a new estimate of relative pose information, such as Δx,Δy, and Δθ to the observed landmark, from the Visual Front End 602 orthe optional Pre-Filter module 622. The SLAM module 604 uses the changein pose information to update the one or more poses and maps 620maintained. Accordingly, the visually observed landmarks canadvantageously compensate for drift in dead reckoning measurements.

An example of a process for performing SLAM will be described in greaterdetail later in connection with FIG. 13.

Multiple-Hypothesis VSLAM

FIG. 7A illustrates multiple particles in a multiple-hypothesis VSLAMsystem 600. Multiple particles can advantageously be maintained by theSLAM module 604 such that multiple possible locations and maps can bemaintained in a probabilistic manner. In the illustrated. VSLAM system600, a multi-particle approach is not used in the Visual Front End 602.

A collection of K particles 702 form the multiple hypotheses. The numberof particles K in a collection can vary in a very broad range and candepend on a variety of parameters including environmental conditions,hardware configurations, available CPU resources, and the like. In oneexample, the number of particles K is about 200. Other usable values forthe number of particles K will be readily determined by one of ordinaryskill in the art. In one embodiment, the number of particles K variesadaptively according to at least one of the following: available CPUresources; a density of particles; the number of landmarks encountered;and the like.

In the illustrated architecture, each particle or hypothesis isassociated with a pose and a map. In this manner, multiple hypotheticalposes and multiple hypothetical maps are maintained. For example, afirst particle 704 is associated with a first pose s₁ 706 and a firstmap M₁ 708. In one embodiment, particles that are associated with arelatively low probability of being correct are discarded, and newparticles are introduced into the collection of K particles 702 bycopying other particles. Perturbations, such as random perturbations, tothe particle poses, which will be described later in connection withFIG. 13, permit copied particles to deviate from the original particles.

FIG. 7B illustrates one example of data associated with a hypothesis orparticle 710. The hypothesis or particle 710 can be associated with arobot pose s 712 and a map M 714. The robot pose s 712 corresponds tothe pose (location and orientation) of the robot as described earlier inconnection with FIG. 4. In the illustrated format, the data for therobot pose s 712 is represented by a 4×1 column vector including a lastupdate time t, an x-position x, a y-position y, and a heading θ. Thelast update time t can be used by the SLAM module 604 to referenceappropriate data from the dead reckoning data to compute the distanceand course traveled when updating the robot pose s 712. It will beunderstood that the robot pose s 712 can be stored in a variety offorms, such as in polar coordinates and in other vector or non-vectorrepresentations.

The map M 714 includes a collection of landmarks 716. In this context,it will be understood that “landmark” refers to data in the collectionthat is associated with a physical landmark in the environment. Itshould be noted that a physical landmark can correspond to one or moreobjects in the environment. The number of landmarks J in the collection716 can vary in a very broad range. Initially upon startup, the numberof landmarks J can correspond to zero and can grow as landmarks areobserved by the robot in the environment. Of course, the number oflandmarks J in the collection 716 can vary according to the density orclutter of identifiable landmarks in the environment and the size of theenvironment. For example, in a test using a fairly typical apartment asa test environment, the number of landmarks J in the collection 716 hasbeen observed in the range of about 10 to about 100. In addition, thecriteria applied to determine whether visual features can be reliablydetected by the Visual Front End 602 can also affect the number oflandmarks J stored in the collection 716.

In the illustrated structure, a landmark is associated with a landmarktag or identifier I, a landmark pose estimate S, and an uncertaintymeasure, such as, for example, a covariance matrix C. Informationdescribing the visual characteristics or image of the landmark, such as3-D features, can be stored in a collection of data associated with theVisual Front End 602, such as in the landmark database 606. In acollection of data for the SLAM module 604, such as the SLAM database608, a cross reference or database record identifier can be used toidentify the landmark tag I. An example of a data structure for the SLAMdatabase 608 and methods for efficiently managing VSLAM databases willbe described later in connection with FIG. 8.

It should be noted that the landmark pose S corresponds to the pose ofthe robot itself when the robot “creates” the landmark and adds thelandmark to the map. In one embodiment, the landmark pose S can also beupdated when the robot re-observes the landmark. In the illustratedstructure, the landmark pose S corresponds to a 3×1 column vector withthe contents of an x-dimension x for global reference, a y-dimension yfor global reference, and a robot heading θ relative to the globalreference frame. As noted earlier, the hypothetical pose and thecorresponding map can advantageously vary among the particles of amulti-particle or multiple hypothesis SLAM system.

A covariance matrix C represents the uncertainty of the landmark pose S.The symbol C_(m) ^(k) will be used herein to denote the covariancematrix associated with landmark k for particle m. In one embodiment, thecovariance matrix C_(m) ^(k) is updated with a Kalman filter as will bedescribed in greater detail later in connection with FIG. 13.

Example of a Data Structure

FIG. 8 illustrates one example of a data structure for a relationaldatabase that can be used with an embodiment of the invention tomaintain data for visual localization and for SLAM.

It will be understood by one of ordinary skill in the art that adatabase can be implemented on an addressable storage medium and can beimplemented using a variety of different types of addressable storagemediums. For example, the landmark database 606 and/or the SLAM database608 can be entirely contained in a single device or can be spread overseveral devices, computers, or servers in a network. The landmarkdatabase 606 and/or SLAM database 608 can be implemented in such devicesas memory chips, hard drives, optical drives, and the like. Though thedata structure shown has the form of a relational database, one ofordinary skill in the art will recognize that the database may also be,by way of example, an object-oriented database, a hierarchical database,a lightweight directory access protocol (LDAP) directory, anobject-oriented-relational database, and the like. The databases mayconform to any database standard, or may even conform to a non-standard,private specification. The database can also be implemented utilizingany number of commercially available database products such as, by wayof example, Oracle® from Oracle Corporation, SQL Server and Access fromMicrosoft Corporation, Sybase® from Sybase, Incorporated and the like.

The data structures shown utilize a relational database managementsystem (RDBMS). In a RDBMS, the data is stored in the form of tables.Conceptually, data within the table is stored within fields, which arearranged into columns and rows. Each field contains one item ofinformation. Each column within a table is identified by its column nameand contains one type of information, such as a value for a SIFTfeature. For clarity, column names are illustrated in the tables of FIG.8.

A record, also known as a tuple, contains a collection of fieldsconstituting a complete set of information. In one embodiment, theordering of rows does not matter as the desired row can be identified byexamination of the contents of the fields in at least one of the columnsor by a combination of fields. Typically, a field with a uniqueidentifier, such as an integer, is used to conveniently identify arelated collection of fields.

By way of example, three tables are shown for a landmark database datastructure 800, and three tables are shown for a SLAM database datastructure 810. The exemplary data structures illustrate a convenient wayto maintain data such that an embodiment using the data structures canefficiently store and retrieve the data therein. The tables for thelandmark database data structure 800 include a Landmark Table 802, aFeature Table 804, and an optional Object Recognition Table 806.

The Landmark Table 802 stores data related to when a landmark was“created,” that is, when a visual landmark was recognized as a “new”landmark and added to the landmark database 606. A Landmark ID field canbe used to identify the appropriate field for a particular landmark. ACreation Timestamp field can be used to store an indication of when thelandmark was observed by the robot and then added to the database. TheCreation Timestamp field can be filled with a timestamp from, forexample, a computer's operating system time. In one embodiment, theLandmark Table 802 is optional.

The Feature Table 804 stores data relating to the identification of alandmark and data useful for calculating a relative pose. For example, alandmark can be characterized by a plurality of 3-D features. TheFeature Table 804 includes fields for a Landmark ID, a Feature ID foreach 3-D feature stored, a Feature descriptor associated with each 3-Dfeature stored, the 3-D coordinates of each 3-D feature in the landmarkreference frame, and optionally, the 2-D coordinates of the 2-D featureassociated with each 3-D feature. The Landmark ID field can be used toidentify the records that correspond to a particular landmark. A visuallandmark is typically characterized by a number of 3-D features.Accordingly, the Feature ID field can be used to identify records thatcorrespond to a particular feature of a landmark. The Landmark ID fieldfor a record can be used to identify the particular landmarkcorresponding to the feature and be used to identify related records forother features of the landmark. The Feature descriptor field can be usedto store visual information about the feature such that the feature canbe readily identified upon a revisit of the landmark by the robot. The3-D coordinate of feature field can be used to store the 3-D coordinatesof the corresponding 3-D feature in the landmark reference frame. Thisprovides a reference for projection calculations which are later usedwhen calculating the relative pose when the landmark is re-encountered.For example, where a camera is used as the visual sensor, the 2-D imagecoordinates can include one or more pixel locations that correspond tothe 3-D features.

The optional Object Recognition Table 806 includes the Featuredescriptor field, the Landmark ID field, and the Feature ID field. Theoptional Object Recognition Table 806 can advantageously be indexed bythe Feature descriptor, which can facilitate the matching of observedimages to landmarks.

The tables for the SLAM database data structure 810 include the DeadReckoning Data Matrix 812, a Multiple Hypothesis Table 814, and a MapTable 816. The Dead Reckoning Data Matrix 812 can include a field fordead reckoning data (x, y, θ) and a field for a timestamp. Theinformation stored in the Dead Reckoning Data Matrix 812 can be used toestimate course and distance traveled from a previous point. In oneembodiment, the Dead Reckoning Data Matrix 812 accumulates deadreckoning data on an ongoing basis. Although dead reckoning is typicallyrelatively accurate over short distances, calibration errors, slippage,and the like can eventually accumulate in the dead reckoning data asillustrated by the second path 206 described earlier in connection withFIG. 2.

The Multiple Hypothesis Table 814 includes a Hypothesis ID field, whichserves as an identifier for a hypothesis or particle in amulti-hypothesis SLAM system. A robot pose field stores the robot pose(x, y, θ), and an update time field stores a time corresponding to thelast update time (t) for the particle. In one vector representation,which will be described later, the pose S includes both the pose (x, y,θ) and the last update time (t).

The Map Table 816 includes a Hypothesis ID field that identifies thehypothesis to which a particular map belongs. As described earlier inconnection with FIG. 7B, a map includes one or more landmarks. ALandmark ID field identifies which records in the map table correspondto a particular landmark. In one embodiment, the same value for theLandmark ID field is used in the Landmark Table 802, in the FeatureTable 804, and in the Map Table 816 for a particular landmark, but itwill be understood by one of ordinary skill in the art that suchidentifying values can be also translated or cross referenced acrossdisparate identifiers. A Last Observed Time field can store a timestampthat indicates the last time that the corresponding landmark informationwas updated. A Landmark Pose field stores a globally-referenced robotpose corresponding to when the Landmark was “created” or added, orupdated to the database. It should be noted that the Landmark Pose forthe “creation” pose can also be updated or refined as furthermeasurements are made by the robot. A covariance field stores data for acovariance matrix associated with the landmark. In one embodiment, thecovariance matrix is updated by a Kalman filter as will be describedlater in connection with FIG. 13.

The following initialization parameters can be used for the VSLAM systemwhen the robot is started or reset or when the VSLAM system is reset.For example, when the robot is first powered up, the followinginitialization parameters can be used. With respect to the landmarkdatabase 606, the Landmark Table 802 and the Feature Table 804 can beinitialized to be empty or cleared. With respect to the SLAM database608, the Dead Reckoning Data Matrix 812 can be initialized such thatx=0, y=0, θ=0, and such that the timestamp field corresponds to thecurrent time for the robot. Where implemented in a personal computer,such as a laptop computer, the current time can typically be retrievedfrom the operating system.

Each hypothesis in the Multiple Hypothesis Table 814 can be initializedby setting a particle's pose to x=0, y=0, θ=0, and by setting the updatetime field for each particle to the current time. For initialization,the Map Table 816 can be cleared.

Management of Databases

It will be understood by the skilled practitioner that the size of thedatabases holding the various maps for the particles can grow over timeas landmarks are accumulated in the maps. One embodiment of theinvention also include techniques for managing the databases.

The landmark database 606 and the SLAM database 608 can be managed toprovide efficient performance of VSLAM processing in a diverse varietyof settings and to manage the amount of memory used in VSLAM processing.One way to efficiently manage the databases is to remove landmarks fromthe databases that are perceived to be no longer present in theenvironment or can otherwise be considered unreliable, bad, or in anyother way undesired.

For example, the assessment that a physical landmark has disappearedfrom the environment such that the corresponding landmark should beremoved from the databases can be based on repeatedly not observing thephysical landmark at or near poses where it is expected to be observed.

In another example, measurements that repetitively correspond tooutliers, i.e., relatively extreme measurements, can also be consideredto be unreliable and can be removed from the databases. For example, alandmark can be considered to be unreliable or bad if measurements ofthe landmark over time have been repeatedly inconsistent or otherwiseindicated as unreliable. An example of a range for repeatedlyinconsistent measurements is about 5 to 10 inconsistent measurements.Other appropriate values will be readily determined by one of ordinaryskill in the art. In one embodiment, a measurement for a landmark isinconsistent if the measurement suggests that the robot is locatedrelatively far away from where a relatively large proportion of theparticles, such as about 90%, the SLAM subsystem predicts the robot tobe. In one embodiment, the robot is determined to be located relativelyfar away when the SLAM prediction prior to incorporation of the newvisual measurement into an estimate falls outside a 95% confidenceellipse. In one embodiment, the 95% confidence ellipse has (i) thevisual measurement estimate of robot pose as its mean, and (ii)C_(sensor) as its covariance matrix. In another embodiment, the robotcan be determined to be located relatively far away when the differencebetween the pose estimated by SLAM and the pose estimated by the visualmeasurement exceed a predetermined threshold. An example of anappropriate value for a predetermined threshold in an indoor environmentis about 2 meters. Other values will be readily determined by one ofordinary skill in the art. It should be noted that while “repeatedlyinconsistent” measurements for a landmark can indicate that the landmarkis unreliable, an occasionally inconsistent measurement may or may notindicate that the landmark is unreliable, but rather, such occasionallyinconsistent measurements may be the result of collisions of the robotwith another object, a “kidnapping” of the robot, such as by lifting andmoving the robot from one spot to another, and the like. In oneembodiment, such occasionally inconsistent measurements do not result ina deletion of the landmark from the databases.

In another example, landmarks can be considered undesirable when, forexample, it is determined that the density of landmarks in some parts ofthe map is relatively high, such as about 5-10 landmarks per squaremeter for an indoor environment. It will be understood that the densityof landmarks can vary considerably from one environment to another andthat correspondingly, appropriate thresholds for “high” density willalso vary and will be readily determined by the skilled practitioner. Byselectively removing some of the landmarks in a too dense portion of themap, memory can be freed for other tasks.

In one embodiment, a memory management technique can be used when thelandmark database has grown to a relatively large size. Typically, amass storage device such as a hard disk is relatively slow compared to asolid-state memory device, such as random access memory (RAM).Conversely, a mass-storage device typically has much more storagecapacity than a solid-state memory device. Alternatively, a solid-statememory device, such as, for example, a flash memory or an EEPROM device,can be used to store a landmark database in a non-volatile manner.Memory usage can be efficiently managed by maintaining only a relativelysmall fraction of the total landmark database in the relatively fastmemory, such as the RAM, at a time. For example, a few initial landmarkmeasurements and comparisons with the landmark database can typicallyreveal approximately where the robot is likely to be operating in amapped environment. For example, an entire house, office, or hospitalfloor can be mapped as the environment; and after a few initialmeasurements, the VSLAM system 600 can determine that the robot is in aparticular room in a house, on the first floor of an office, in aparticular wing of a hospital, and the like.

To reduce the consumption of memory resources, at least partly inresponse to the determination of the approximate location of the robot,the VSLAM system 600 can then maintain a relatively small subset of thedatabase in RAM that contains the relevant portion of the database, andother previously used memory resources can be released back to thesystem. Should, for example, a relatively long period of time transpirewithout successful matches with the loaded subset of the database, theentire map can again be loaded temporarily to determine if the robot hasbeen moved or has moved to another part of the environment. For example,the robot may have traveled autonomously or may have been picked up andmoved to a new location.

In one embodiment, the subset of the map that is maintained inrelatively fast memory such as RAM can at least temporarily correspondto a randomly selected subset of the plurality of landmarks from themap. In another embodiment, the subset of the map that is maintained inrelatively fast memory can at least temporarily correspond to a subsetthat is selected such that the density of landmarks remaining in thesubset is relatively uniformly distributed throughout the map. Thesetechniques can advantageously be used, for example, to assist a mobilerobot with relatively limited memory resources and/or computationalresources to localize itself within one or more maps.

In one embodiment, the VSLAM system advantageously discontinues addingnew landmarks to the database. In one example, the VSLAM systemdiscontinues landmark creation in a portion of an environment or in theentire environment at least partly in response to a determination thatthe landmark density has exceeded a predetermined threshold, such as5-10 landmarks per square meter. For example, when a database for anenvironment exhibits relatively high landmark density in one portion ofthe environment and a relatively low landmark density in another portionof the environment, the addition of new landmarks to the database can bedisabled for the portion of the environment corresponding to therelatively high landmark density in the database, and the addition ofnew landmarks can be enabled for portions of the environmentcorresponding to the relatively low landmark density.

In one embodiment, the VSLAM system discontinues adding new landmarks tothe database at least partly in response to a landmark creation decayrate, i.e., a determination that over a period of time, fewer and fewernew landmarks are being identified. The measurement of the landmarkcreation decay rate can be applied to parts of an environment or to theentire environment. For example, in a relatively static environmentunder relatively constant lighting conditions, the rate at whichlandmarks are created will typically be highest in the beginning, beforemany landmarks have been created. After the area has been partiallymapped by the creation of landmarks, i.e., the addition of landmarks tothe database, the visual front end less frequently attempts to createlandmarks. In one embodiment, a creation rate corresponds to the numberof landmarks created per meter of travel. When the creation rate in agiven part of the environment has dropped below a threshold, which cancorrespond to for example, (i) a predetermined value such as 1 landmarkevery 10 meters, or can correspond to (ii) a percentage of the initialcreation rate such as 5% of the rate (per unit of distance traveled)obtained during the first passage through the relevant part of theenvironment, then landmark creation can be temporarily discontinued inthat part of the environment.

In another embodiment, the VSLAM system discontinues adding newlandmarks to the database for all or part of the environment at leastpartly in response to a ratio of visual measurements to landmarkscreated. In another embodiment, the discontinuing of adding landmarkscan also be triggered at least partly in response to elapsed time, suchas after 30 minutes of mapping, or at least partly in response to memoryusage, such as when 75% of the memory has been used for the landmarkdatabase.

Visual Processing in a Visual Front End

Various processes will now be described. Processes used when revisitingpreviously detected landmarks will first be described. Processes usedwhen adding newly detected landmarks to the database will then bedescribed. These processes can be used to build a map autonomously.These processes can be used to refine existing maps and to localize therobot within one or more maps.

FIG. 9 is a flowchart that generally illustrates a process that can beused in a visual front end for visual processing. As a robot with VSLAMmoves in an environment, the robot analyzes the physical landmarks thatit observes. Recognized landmarks can be used to localize the robotwithin one or more maps. Newly-created landmarks can be added to one ormore maps. The act of creating a new landmark may also be referred to as“generating” a landmark. It will be appreciated by the skilledpractitioner that the illustrated process can be modified in a varietyof ways without departing from the spirit and scope of the invention.For example, in another embodiment, various portions of the illustratedprocess can be combined, can be rearranged in an alternate sequence, canbe removed, and the like. In addition, it should be noted that theprocess can be performed in a variety of ways, such as by softwareexecuting in a general-purpose computer, by firmware executed by amicroprocessor, by dedicated hardware, and the like.

The process begins at a state 902, where the process retrieves an imagefrom the visual sensor or applicable data buffer. At this point in theprocess, multiple viewpoints are not used. For example, where the visualsensor for the robot corresponds to multiple cameras, one image from thecamera can be selected for analysis. It will be understood that theimage can also be related to a timestamp, which can permit otherprocesses to reference appropriate data from the dead reckoning sensorsto the image. The process advances from the state 902 to a state 904.

In the state 904, the process generates a list of matching landmarks.For example, the process can extract feature descriptors from the image,such as SIFT feature vectors, and compare the extracted features tofeatures for landmarks that had previously been observed and stored. Forexample, features for landmarks can be stored in the landmark database606. In one embodiment, the optional Object Recognition Table 806 isused for relatively fast searching of feature descriptors. In oneembodiment, the landmarks with one or more matching landmarks areidentified by a list, such as a list of landmark identifiers, forfurther analysis. The process advances from the state 904 to thebeginning of a loop 906.

The loop further compares the features of the matching landmarksidentified by the list to features of the observed image. It will beunderstood that where no matching landmarks are identified in the state904, the process can skip the execution of the loop and proceed to theend of the loop 916.

For each matching landmark, the loop begins at a decision block 908. Inthe decision block 908, the process compares the number of features thatmatch between the image and the matching landmark. The process uses thedetected features to estimate a relative pose to the landmark. In oneembodiment, the number of features detected is compared to apredetermined number corresponding to the minimum number of featuresneeded to solve the structure and motion problem. It will be understoodthat the minimum number of features can depend on the technique used tosolve the structure and motion problem. For example, where the structureand motion problem is resolved using the trifocal tensor method, theminimum number of features for convergence of a solution is about 5. Theprocess proceeds from the decision block 908 to a state 910 when thereare enough matching features for the landmark. Otherwise, the processproceeds to the end of the loop 916 to return to process furtherlandmarks or to exit out of the loop.

In the state 910, the process computes the camera pose of the robot withrespect to the landmark reference frame. The camera pose corresponds tothe relative pose, such as Δx, Δy, and Δθ, between the posecorresponding to the image retrieved in the state 902 and the landmarkpose for the matched landmark. It will be understood that the relativepose computation can include further dimensions, such as a change invertical component (Δz), roll, and pitch, and that the relative pose canalso be represented in other coordinate system forms.

An example of the relative pose was described earlier in connection withFIGS. 5B, 5C, and 5D. The skilled practitioner will appreciate that manytechniques can be used to compute the relative pose. Onecomputationally-efficient technique to compute the relative pose is tocalculate the relative pose that results in a relatively smallprojection error, such as the minimum projection error.

In one embodiment, the process retrieves the 3-D coordinates for thefeatures of the landmark from a data store, such as from the FeatureTable 804 of the landmark database 606. From the 3-D coordinates, theprocess shifts a hypothetical pose (relative to the landmark pose) andcalculates new 2-D image coordinates by projection from the 3-Dcoordinates and the change in pose. In one embodiment, the relative poseis determined by searching in a six-dimensional 3-D pose space, such as,for example, x, y, z, roll, pitch, and yaw (θ) for a point with arelatively small root mean square (RMS) projection error between thepresently-measured feature coordinates and the projected coordinatesfrom the 3-D feature to the image. The process advances from the state910 to a decision block 912.

Iterative computations for finding numerical solutions can be used tocompute the relative pose. It should be noted that such techniques donot always converge to a result. When convergence is achieved, that is,the landmark match is relatively good, the process proceeds from thedecision block 912 to a state 914 to store information relating to thematched landmark. Otherwise, the process proceeds from the decisionblock 912 to the end of the loop 916. Of course, it will be understoodthat other techniques, such as relatively computationally inefficientbrute force techniques, can also be used to calculate a relative posewith a relatively small projection error.

In the state 914, the process stores results relating to the matchedlandmark such that the results can be provided to a pre-filteringprocess and/or directly to SLAM processes. In one embodiment, the matchresults for a landmark include the landmark identifier for the evaluatedlandmark, the calculated camera pose, a root mean square (RMS) value ofthe projection error for the calculated camera pose, the number ofmatching features, and a computation of slope. In one embodiment, thestoring of one or more of the computation of the RMS value of theprojection error, the number of matching features, and the computationof slope is optional and is provided when one or more of these metricsare used by pre-filtering processes, which will be described in greaterdetail later in connection with FIG. 12. The process can store thesemetrics such that later re-computation of one or more of the results canadvantageously be avoided. The process advances from the state 914 tothe end of the loop 916, where the process returns to the beginning ofthe loop 906 to process further matching landmarks or proceeds to adecision block 918 when done with processing matching landmarks.

In the decision block 918, the process determines whether there has beenat least one converging solution to solving for the relative pose orcamera pose, e.g., Δx, Δy, and Δθ. For example, in one embodiment, theprocess determines whether there has been at least one convergingsolution when the process had proceeded from the decision block 912 tothe state 914 for at least one of the landmarks that were identified tobe matching.

When there has been at least one convergence, this indicates that therehas been at least one relatively “good” match between what was observedby the robot and at least one of the landmarks in the database, and theprocess proceeds from the decision block 918 to a state 920. Otherwise,the process proceeds from the decision block 918 to a state 922.

In the state 920, the process provides the match results previouslystored in the state 914 to Pre-Filtering processes and/or to SLAMprocesses such that the matched landmarks can be used to localize therobot within the global reference frame. Advantageously, thisinformation can be used by a SLAM process to correct for drift in thedead reckoning information. The match results can include match resultsfor one or more landmarks. When a plurality of landmarks are identifiedin a single image, one embodiment of the SLAM process can process all ofthe plurality as will be described later in connection with FIG. 13.

Returning now to the state 922, at this point in the process, theprocess has determined that there are no relatively “good” matchesbetween what was observed by the robot and the landmarks in thedatabase, and the process proceeds to landmark creation processes, suchas the process that will be described in greater detail later inconnection with FIG. 10.

Visual Front End Processing for Landmark Creation (New Landmarks)

FIG. 10 is a flowchart that generally illustrates a process that can beused in a visual front end when recognizing a new physical landmark inthe environment and creating a corresponding landmark in one or moremaps in a database. The act of creating a new landmark may also bereferred to as “generating” a landmark. It will be appreciated by theskilled practitioner that the illustrated process can be modified in avariety of ways without departing from the spirit and scope of theinvention. For example, in another embodiment, various portions of theillustrated process can be combined, can be rearranged in an alternatesequence, can be removed, and the like. In addition, it should be notedthat the process can be performed in a variety of ways, such as bysoftware executing in a general-purpose computer, by firmware executedby a microprocessor, by dedicated hardware, and the like.

The process begins at a state 1002, where the process retrieves a groupof at least 2 images for analysis. For example, the images can beprovided by an visual sensor with multiple images, such as a binocularor trinocular camera, or by a visual sensor with a single imager, suchas from a single camera. When images from a single camera are used, theprocess can select images that are appropriately spaced apart. In theillustrated embodiment, the robot is equipped with a singleforward-looking camera and travels forward to take related images. Otherconfigurations for the visual sensor are also possible. In otherexamples, the visual sensor can correspond to a generallyupward-pointing camera, to a sideways-looking camera, or to positionsbetween forward looking, upward, and/or sideways. Returning now to theillustrated embodiment with a single forward-looking camera, in oneexample, three images are selected at a separation distance of at leastabout 10 centimeters (cm) apart. It will be understood that anappropriate distance for the separation distance can vary in a broadrange depending on the environment. For example, where the operatingenvironment corresponds to a relatively expansive environment, such asto an outdoor environment, the appropriate distance for separationbetween images can be higher in order to gain perspective on thefeatures. In one embodiment, the separation distance can be adaptivelyvaried in response to an estimate of the proximity of obstacles that canobstruct the motion of the robot. In one embodiment, where the robot isequipped with a single forward-looking camera, the robot moves in anapproximately straight line in the forward direction while taking theimages. Although some turning can be tolerated while the robot is takingimages, the turning should not be so excessive such that the features ofthe landmarks are no longer in the view of the camera. The processadvances from the state 1002 to an optional state 1004.

The state 1004 and a decision block 1006 can be optional depending onthe configuration of the robot. Where the robot is equipped with avisual sensor with multiple imagers, such as a trinocular camera, thestate 1004 and the decision block 1006 can be skipped, and the spacingbetween the visual sensors can be retrieved from a stored parameter inmemory. When skipped, the process advances from the state 1002 to astate 1010.

When a single camera is used as the visual sensor, and the robot movesto take different images from different perspectives, the processretrieves the actual distances between images in the state 1004 andchecks the amount of movement in the decision block 1006. In oneembodiment, these distances are determined by monitoring the deadreckoning data corresponding to the times at which the images weretaken. The process advances from the state 1004 to the decision block1006.

In the decision block 1006, the process tests the distance traveledbetween images, termed “baseline.” For example, the amount of baselinebetween images can be compared to a predetermined value. It will beunderstood that the predetermined value can vary in a very broad range.In an indoor environment, such as the interior of a home or apartment,an appropriate value can be about 10 centimeters for the predeterminedvalue. Of course, the appropriate value can depend on the environment,and other appropriate values will be readily determined by one ofordinary skill in the art. When the movement of the robot is notsufficient between one or more of the images in the group, the processproceeds to a state 1008, and the process does not create a landmark.Otherwise, the process proceeds from the decision block to the state1010.

In the state 1010, the process analyzes the selected images to identify2-D features that are common to the images in the group. The number offeatures that are observable will vary according to the environment. Theextraction of suitable features has been extensively described in theliterature. SIFT features are one example of such 2-D features. See, forexample, David G. Lowe, Local Feature View Clustering for 3D ObjectRecognition Proceedings of the IEEE Conference on Computer Vision andPattern Recognition, Kauai, Hi. (December 2001). It should be noted thatother information used in the calculation of the features can includeintrinsic camera calibration parameters and extrinsic camera parameters.Examples of intrinsic camera calibration parameters include opticalcenter, distortion parameters, and focal length. Examples of extrinsiccamera calibration parameters include a camera-rigid transformationbetween the camera reference frame and the local reference frame. Theprocess advances from the state 1010 to a decision block 1012.

In the decision block 1012, the process determines whether enoughfeatures have been identified that are common to the images in thegroup, e.g., the three images, for reliable identification of thelandmark. When, for example, the process determines that fewer than apredetermined number of features are common to the images in the group,the process can determine that there are not enough features detected toreliably identify the landmark in the future. In this case, the processcan proceed from the decision block 1012 to the state 1008, and theprocess does not “create” a landmark. It will be understood that anappropriate value for the predetermined number of features can vary in avery broad range and can depend on the method used to identify visualfeatures. In one embodiment, the predetermined number of features ishigher for the decision block 1012 for landmark creation than apredetermined value used to compare an image to an already storedlandmark as described earlier in connection with the decision block 908of FIG. 9.

In one embodiment, where SIFT features are used, an example of a samplevalue for the predetermined number of features is about 10. Othersuitable values will be readily determined by one of ordinary skill inthe art. In one embodiment, the VSLAM system 600 can be configured topermit predetermined values to be user configurable. The processproceeds from the decision block 1012 to a state 1014 when enoughfeatures common to the images in the group have been identified.

In the state 1014, the process computes 3-D local reference framepositions or displacements to the common features identified in thestate 1010. In one embodiment, the 3-D local reference frame positionscorrespond to the approximate 3-D position (x, y, z) of a featurerelative to the visual sensor of the robot. Where multiple images aretaken from a single visual sensor as the robot moves, the 3-D localreference frame positions can be relative to the position of the robotwhen the robot took one of the images in the group, such as the firstimage in the group. In one example, the computations for the 3-Dpositions are resolved by solving the structure and motion problem usingthe trifocal tensor method. It will be understood that the features canoccupy a space larger than a point, such that the corresponding 3-Dpositions can be relatively approximate. The process advances from thestate 1014 to a decision block 1016.

In the decision block 1016, the process determines whether there havebeen enough 3-D local reference frame positions for features resolved inthe state 1014 for the landmark to be reliably recognized. It will beunderstood that occasionally, the process may not find a solution to the3-D local reference frame positions for a particular feature such thatthe number of 3-D features with corresponding displacement informationcan be different than the number of such initially-detected features.For example, in the decision block 1016, the process can compare a countof the 3-D local reference frame positions resolved for features of alandmark to a predetermined number. In one embodiment, where SIFTfeatures are used, the process determines that a landmark has asufficient number of features with 3-D local reference frame positionsresolved for relatively reliable recognition when there have been 10 ormore such features resolved. Other appropriate values will be readilydetermined by one of ordinary skill in the art. The process proceedsfrom the decision block 1016 to a state 1018 when the landmark has beendetermined to be reliably recognized. Otherwise, the process proceedsfrom the decision block 1016 to the state 1008, and the process does not“create” a landmark.

In the state 1018, the process identifiably stores the features, the 3-Dpositions, and, optionally, the approximate 2-D image locationscorresponding to the features for the image that is used as thereference. For example, the 3-D position and the 2-D image location fora feature can be stored in a record in the Feature Table 804 describedearlier in connection with FIG. 8. It will be understood that eachlandmark that is created should have a unique reference, such as aunique numbered identifier, and that each feature of a reference shouldalso be identifiable, such as by a combination of the reference for thelandmark, e.g., Landmark ID, and the reference for the feature, e.g.,Feature ID. The process advances from the state 1018 to a state 1020.

In the state 1020, the process provides an indication that a newlandmark has been created to other processes, such as processes relatedto the SLAM portion of VSLAM. An example of a SLAM process for creationof a landmark will be described later in connection with FIG. 11. Forexample, the indication can be provided as a parameter or flag in asoftware call, a hardware or a software interrupt, and the like. Theindication can also be accompanied by the landmark identifier for thenewly created landmark.

SLAM Processing for New Landmarks

FIG. 11 is a flowchart that generally illustrates a process that can beused in a SLAM module when creating a new visual landmark. It will beappreciated by the skilled practitioner that the illustrated process canbe modified in a variety of ways without departing from the spirit andscope of the invention. For example, in another embodiment, variousportions of the illustrated process can be combined, can be rearrangedin an alternate sequence, can be removed, and the like. In addition, itshould be noted that the process can be performed in a variety of ways,such as by software executing in a general-purpose computer, by firmwareexecuted by a microprocessor, by dedicated hardware, and the like. Aprocess that can be used in a SLAM module when returning to an existinglandmark will be described later in connection with FIG. 13. In a SLAMmodule, a landmark is represented by the pose of the landmark referenceframe in the global reference frame, i.e., the transformation (rotationand translation) that relates the landmark reference frame to the globalreference frame.

Returning to the process illustrated in FIG. 11, the process begins at astate 1102, where the process receives an indication that a new landmarkhas been observed. For example, the process can receive the indicationof the new landmark and a corresponding identifier for the new landmarkfrom the state 1020 of the process described earlier in connection withFIG. 10. The process advances from the state 1102 to a state 1104.

In the state 1104, the process computes the change in pose from a lastupdate time for the SLAM system. In one embodiment, all the particles ofa SLAM system are updated at the same time such that the last updatetime for a particular particle is the same as the last update time forthe other particles.

The change in pose is computed by retrieving data provided by the deadreckoning sensors and/or interface. In one embodiment, the processretrieves the appropriate data from a data store, such as from adatabase including the Dead Reckoning Data Matrix 812 described earlierin connection with FIG. 8. For example, a timestamp associated with thelast update time for the particles and a timestamp associated with therecognition of the observed landmark can be used to identify theappropriate data to be retrieved from the Dead Reckoning Data Matrix812. In one embodiment, the process computes a change in pose [Δ₁, Δ₂,Δ₃]^(T) from the dead reckoning data, the results of which will be usedlater to estimate or predict the pose of a robot in the global referenceframe and/or update the landmark pose.

$\begin{matrix}{\Delta^{odom} = {\begin{bmatrix}\Delta_{1}^{odom} \\\Delta_{2}^{odom} \\\Delta_{3}^{odom}\end{bmatrix} = \begin{bmatrix}\sqrt{\left( {y_{l} - y_{k}} \right)^{2} + \left( {x_{l} - x_{k}} \right)^{2}} \\{\left\lbrack {\left( {{\arctan\left( \frac{y_{l} - y_{k}}{x_{l} - x_{k}} \right)} - \theta_{k} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi} \\{\left\lbrack {\left( {\theta_{l} - \theta_{k} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}\end{bmatrix}}} & {{Equation}\mspace{14mu} 1}\end{matrix}$

In Equation 1, the change in pose from a first dead reckoning pose attime k (x_(k),y_(k),θ_(k)) to a second dead reckoning pose at time l(x_(l),y_(l),θ_(l)) is computed. In one embodiment, the change in poseis computed by a function call, such as a call to a “DeltaPose”subroutine. A variable Δ₁ ^(odom) corresponds to the Euclidean distancebetween (x_(k),y_(k)) and (x_(l),y_(l)). A variable Δ₂ ^(odom)corresponds to the bearing from the robot at time k to the robot at timel. A variable Δ₃ ^(odom) represents the change in heading from the robotat time k to the robot at time l. The “mod” denotes the arithmeticmodulus operator. The process advances from the state 1104 to thebeginning of a loop 1106.

Where multiple particles are used to track multiple hypothesis, the loopupdates each particle that is maintained. The loop starts at a state1108. In the state 1108, the process retrieves the landmark identifierfor the newly defined landmark. In one embodiment, the same landmarkidentifier is used to identify a landmark in a SLAM process as theidentifier for the landmark in a visual localization process. Of course,a different landmark identifier can also be generated andcross-referenced. It should also be noted that the SLAM process does notneed to store graphical information, such as 3-D features, of thelandmark. Rather, the SLAM process can operate by identification ofwhich landmark was encountered, such as the Landmark ID, such that adatabase record identifier can be used to identify the landmarks withinSLAM. The process advances from the state 1108 to a state 1110.

In the state 1110, the process adds the new landmark pose to thedatabase. It should be noted that in the illustrated embodiment, theinitial estimate of the new landmark pose is the estimated pose of therobot corresponding to when the landmark was observed that is stored inthe database and not the estimated position in space of the physicallandmark itself. To add the new landmark pose to the database, theprocess estimates the current pose of the robot for the particlecorresponding to the particular iteration of the loop. In oneembodiment, the current pose is estimated by combining the change inpose from the dead reckoning data as calculated in the state 1104 withthe previous pose of the robot for the particle as retrieved from thelast time that the particle was updated. Equation 2 expresses one way tocombine the change in pose [Δ₁ ^(odom), Δ₂ ^(odom), Δ₃ ^(odom)]^(T) witha previous pose (x_(k), y_(k), θ_(k)) to generate a new pose (x_(l),y_(l), θ_(l)), which is used as the new landmark pose. It will beunderstood that the subscripts of k and l as used in the state 1110represent different variables than the same subscripts of k and l asused in the state 1104.

$\begin{matrix}{\begin{bmatrix}x_{l} \\y_{l} \\\theta_{l}\end{bmatrix} = \begin{bmatrix}{x_{k} + {\Delta_{1}^{odom}{\cos\left( {\theta_{k} + \Delta_{2}^{odom}} \right)}}} \\{y_{k} + {\Delta_{1}^{odom}{\sin\left( {\theta_{k} + \Delta_{2}^{odom}} \right)}}} \\{\left\lbrack {\left( {\theta_{k} + \Delta_{3}^{odom} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}\end{bmatrix}} & {{Equation}\mspace{14mu} 2}\end{matrix}$

In one embodiment, the new robot pose (x_(l), y_(l), θ_(l)) is computedby a function call, such as a call to a “PredictPose” subroutine. Theprocess advances from the state 1110 to a state 1112.

In the state 1112, the process initializes the landmark covariancematrix C_(m) ^(k) associated with the particle corresponding to theiteration of the loop, where in is a particle loop variable and where kis a landmark loop variable. In one embodiment, the landmark covariancematrix C_(m) ^(k) is initialized to a diagonal matrix 3×3 matrix. In oneembodiment, the landmark covariance matrix C_(m) ^(k) is initialized toa diagonal matrix of diag(81 cm², 81 cm², 0.076 rad²). Other suitableinitialization values for the landmark covariance matrix C_(m) ^(k) willbe readily determined by one of ordinary skill in the art. The valuesfor the landmark covariance matrix C_(m) ^(k) can be stored in a recordin the Map Table 816. It will be understood that appropriateinitialization values can vary in a very broad range and can depend on avariety of factors, including camera specification, dead reckoningequipment precision, and the like. The process proceeds from the state1112 to the end of the loop 1114. The process returns to the beginningof the loop 1106 when there are remaining particles to be updated.Otherwise, the process ends.

Pre-Filtering Process

FIG. 12 is a flowchart that generally illustrates a process that can beused in a pre-filter of a VSLAM system to remove outliers. In oneembodiment, the Pre-Filtering process is optional. The Pre-Filteringprocess can advantageously be configured to prevent a SLAM module fromreceiving unreliable measurements of landmarks that may or may not havebeen unreliably identified. This can advantageously enhance thestability and robustness of VSLAM. It will be appreciated by the skilledpractitioner that the illustrated process can be modified in a varietyof ways without departing from the spirit and scope of the invention.For example, in another embodiment, various portions of the illustratedprocess can be combined, can be rearranged in an alternate sequence, canbe removed, and the like. In addition, it should be noted that theprocess can be performed in a variety of ways, such as by softwareexecuting in a general-purpose computer, by firmware executed by amicroprocessor, by dedicated hardware, and the like.

In the illustrated process, three tests are used to establish thereliability of an observed landmark. It will be understood that inanother embodiment, fewer tests can be used or other tests can be added.In one embodiment, the tests are based on data or metrics computed bythe Visual Front End 602, and the processing for the pre-filter isperformed in the Pre-Filter module 622. This data can be provided in alist of matching landmarks as described earlier in connection with thestate 920 of the process of FIG. 9. When more than one visualmeasurement is provided in a list, the Pre-Filtering process can berepeated to process each visual measurement in the list. The change inpose is relative to the pose that the robot had when the landmark wascreated.

The process starts at a decision block 1202, where the process evaluatesthe level of the RMS projection error encountered when computing thelandmark pose, e.g., Δx, Δy, and Δθ, relative to the robot. In oneembodiment, the relative pose is estimated by searching in asix-dimensional 3-D pose space for a pose with a relatively small RMSprojection error as described earlier in connection with the state 910of FIG. 9. Advantageously, the RMS projection error does not have to berecomputed by the pre-filter process. In one embodiment using a cameraas the visual sensor, when the RMS projection error is less than about 3pixels, the test for RMS projection error is considered passed. Theprocess proceeds from the decision block 1202 to a decision block 1206when the test is passed. Otherwise, when the test has not passed, theprocess proceeds from the decision block 1202 to a state 1204.

In the state 1204, the measurement of the observed landmark isconsidered to be unreliable, and the process does not use the computedlandmark measurement for updating the robot pose and the map in SLAM. Inone embodiment, this is accomplished by not executing a SLAM process forrelative measurements computed in the visual front end or by notinforming the SLAM module that a previously defined landmark has beenre-encountered. In one embodiment, the Pre-Filtering process effectivelyignores measurements that did not pass one or more tests from the listof landmark measurements provided by the state 920 of FIG. 9. Theprocess then proceeds from the state 1204 to end and can be repeated asdesired for other landmark measurements that have been computed.

Returning now to the decision block 1206, at this point in the process,the landmark measurement has passed the RMS projection error test (whereused). In the decision block 1206, the process analyzes the number offeatures for the image that favorably compared with the features of thelandmark. In one embodiment, the process compares the number to apredetermined threshold. Appropriate values for the predeterminedthreshold can vary in a very broad range. In one example, when thenumber of matching features is at least 10, the landmark measurement haspassed the test for the number of features. The process proceeds fromthe decision block 1206 to a decision block 1210 when the landmarkmeasurement passes the test. The process proceeds from the decisionblock 1206 to the state 1204 and does not use the potentially unreliablelandmark measurement when the landmark does not pass the test.

In the decision block 1210, the process compares the calculated slopefor the floor to a predetermined value as a test for reliability. Thechange in pose that is calculated for the robot can include multipledimensions, such as the six-dimensions of x, y, z, roll, pitch, and yaw.It should be noted that robots come in a variety of shapes and forms.Many land-based robots can travel horizontally along a floor, but arenot configured to raise up or down in the vertical dimension z. Inaddition, many environments are relatively flat, such that the presenceor absence of a sloping floor can be used as an indicator of thereliability of the detection of a landmark. The slope can be computedby, for example, dividing the value calculated for the verticaldimension (Δz) by a change in horizontal dimension, such as Euclideandistance traversed along the floor. An example of such a calculation isexpressed in Equation 3.

$\begin{matrix}{{slope} = {\frac{\Delta\; z}{\sqrt{\left( {\Delta\; x} \right)^{2} + \left( {\Delta\; y} \right)^{2}}}}} & {{Equation}\mspace{14mu} 3}\end{matrix}$

In one embodiment, when the magnitude of the calculated slope is lessthan about 0.1, the test is a pass. Other appropriate values will bereadily determined by one of ordinary skill in the art. In oneembodiment, the testing of the slope can be enabled or disabled viaconfiguration by a user such that a robot that operates in a notrelatively flat environment, or a robot that can move in the verticaldimension, can bypass the slope tests. The process proceeds from thedecision block 1210 to a state 1212 when the result of the test is apass or is bypassed. Otherwise, when the result of the test is a fail,the process proceeds from the decision block 1210 to the state 1204, sothat the potentially unreliable landmark measurement is not used.

In the state 1212, at this point in the process, the landmarkmeasurement has passed the reliability tests, and the process permitsthe use of the landmark measurement. This can be accomplished byproviding the SLAM module with an indication that one or more landmarkshas been observed. In one embodiment, the process permits the observedlandmark data, such as the delta pose information, landmark identifier,and an associated timestamp, to pass from the Visual. Front End 602,through the Pre-Filter module 622, and onto the SLAM module 604. It willbe understood that the process can be repeated when more than onelandmark has been detected such that an entire list of landmarkmeasurements can be processed, and that the measurements not rejected bythe Pre-Filtering process can be processed by the SLAM module. Theprocess then ends and can be repeated again as the robot moves about andre-encounters landmarks.

SLAM Processing for Revisiting Landmarks

FIG. 13 is a flowchart that generally illustrates a process that can beused in a SLAM module when a landmark is re-encountered. The illustratedSLAM process advantageously updates one or more particles withinformation obtained from a combination of one or more visual sensorsand one or more dead reckoning sensors. It will be appreciated by theskilled practitioner that the illustrated process can be modified in avariety of ways without departing from the spirit and scope of theinvention. For example, in another embodiment, various portions of theillustrated process can be combined, can be rearranged in an alternatesequence, can be removed, and the like. In addition, it should be notedthat the process can be performed in a variety of ways, such as bysoftware executing in a general-purpose computer, by firmware executedby a microprocessor, by dedicated hardware, and the like.

The process begins at a state 1302, where the process receives matchinglandmark data. The process can be used with or without a Pre-Filteringprocess, such as the process described earlier in connection with FIG.12. For example, the SLAM process of FIG. 13 can receive unfilteredlandmark measurement data from the state 920 of FIG. 9 or can receivefiltered data from the state 1212 of FIG. 12. The process advances fromthe state 1302 to a state 1303.

In the state 1303, a sub-matrix is extracted from dead reckoning data,such as from the Dead Reckoning Data Matrix 812. In one embodiment, thesub-matrix, denoted D, corresponds to a 3×M matrix as illustrated inEquation 4.

$\begin{matrix}{D = \begin{pmatrix}x_{old} & x_{1} & \ldots & x_{M - 2} & x_{new} \\y_{old} & y_{1} & \ldots & y_{M - 2} & y_{new} \\\theta_{old} & \theta_{1} & \ldots & \theta_{M - 2} & \theta_{new}\end{pmatrix}} & {{Equation}\mspace{14mu} 4}\end{matrix}$

In Equation 4, variables x_(old), y_(old), and θ_(old) correspond to apose according to dead reckoning data from about the time of the lastupdate for the particles t_(old). Variables x_(new), y_(new), andθ_(new) correspond to a pose according to dead reckoning data at aboutthe time t_(new) that the landmark was re-encountered. Variables incolumns 2 through M−1, i.e., x₁, y₁, and θ₁ and x_(M-2), y_(M-2), andθ_(M-2) (and values in-between as applicable), correspond to dataretrieved from the Dead Reckoning Data Matrix 812 at times betweent_(old) and t_(new). In one embodiment, if no data point for the deadreckoning data is available at about times t_(old) and/or t_(new), thevariables x_(old), y_(old), and θ_(old) and x_(new), y_(new), andθ_(new) can be interpolated from surrounding data. The process advancesfrom the state 1303 to a state 1305.

In the state 1305, the process computes the change in pose traveled froma last update time for the SLAM system. In one embodiment, all theparticles of a SLAM system are updated at the same time such that thelast update time for a particular particle is the same as the lastupdate time for the other particles. An example of change in posecomputations were described earlier in connection with the state 1104 ofFIG. 11. The process advances from the state 1305 to a state 1308.

In the state 1308, the covariance for dead reckoning data is computed.The dead reckoning covariance estimates errors in the dead reckoningdata. It will be understood that the dead reckoning covariance can becomputed by a variety of techniques, and that the computation can varydepending on the type of dead reckoning sensor used. In one embodiment,where the dead reckoning sensor is an odometer, the covariance iscomputed as follows and will be denoted “odometer covariance,” anddenoted C_(odom)(D) and corresponds to a 3×3 matrix. While illustratedgenerally in the context of odometry, it will be understood by theskilled practitioner that the principles and advantages described hereinwill be applicable to other forms of dead reckoning sensors such that inanother embodiment, the odometry covariance C_(odom)(D) can correspondto a dead reckoning covariance C_(dr).

The error noise modeled with the odometer covariance C_(odom)(D) modelsthe additional incremental noise in the dead reckoning from a pose A toanother pose B for the values in the data sub-matrix D. In oneembodiment, the odometer covariance C_(odom)(D) corresponds to the finalvalue of a matrix C_(temp), that is, starts from an initial conditionand is iteratively calculated between columns of the data sub-matrix D,for example, iteratively computed M−1 times. The value of the matrixC_(temp) is initialized to a random noise covariance matrix C_(RN) asexpressed in Equation 5.C _(temp) =C _(RN),   Equation 5

In one embodiment, the random noise covariance matrix C_(RN) correspondsto a diagonal matrix diag(2.8 cm², 2.8 cm², 0.0001 rad²). It will beunderstood that appropriate values for the random noise covariancematrix C_(RN) can vary according to the characteristics of the deadreckoning sensor used. Other appropriate values for the random noisecovariance matrix C_(RN) will be readily apparent to one of ordinaryskill of the art.

The process then enters a loop that executes M−1 times. In theillustrated example, a loop variable i starts from 2 and ends at M, andcorresponds to a column in the data sub-matrix D. For each pass throughthe loop, the process computes Equation 6 to Equation 11. In oneembodiment, the loop variable i increments after a pass through theloop, that is, after computing Equation 11 and before returning toEquation 6.

$\begin{matrix}{d_{AB} = \sqrt{\left( {D_{1,i} - D_{1,{i - 1}}} \right)^{2} + \left( {D_{2,i} - D_{2,{i - 1}}} \right)^{2}}} & {{Equation}\mspace{14mu} 6} \\{\phi_{h} = {D_{3,{i - 1}} - D_{3,1}}} & {{Equation}\mspace{14mu} 7} \\{\phi_{r} = {\left\lbrack {\left( {D_{3,i} - D_{3,{i - 1}} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}} & {{Equation}\mspace{14mu} 8} \\{\varphi = {\phi_{h} + \frac{\phi_{r}}{2}}} & {{Equation}\mspace{14mu} 9} \\{G = \begin{bmatrix}{\cos\;\varphi} & {{- \left( {d_{AB}\sin\;\varphi} \right)}/2} \\{\sin\;\varphi} & {\left( {d_{AB}\cos\;\varphi} \right)/2} \\0 & 1\end{bmatrix}} & {{Equation}\mspace{14mu} 10} \\{C_{temp} = {C_{temp} + {d_{AB}^{2}{G\begin{bmatrix}d_{s}^{2} & 0 \\0 & r_{s}^{2}\end{bmatrix}}G^{T}}}} & {{Equation}\mspace{14mu} 11}\end{matrix}$

It will also be understood that the subscripts for data sub-matrix Dindicate a value taken from the row and column specified by thesubscript, with the row appearing first in the subscript. Equation 6computes the Euclidean distance d_(AB) traveled from a point A to apoint B as obtained from the data sub-matrix D. Equation 7 computes arelative heading φ_(h) from a point A to a point B. Equation 8 computesa change in orientation φ_(r). Equation 9 computes an average heading φfrom a point A to a point B. Equation 10 computes a matrix G, which isused to transform a column vector with error information intranslational error and rotational error into another column vector witherror information in x error, y error, and orientation (θ) error.Equation 11 represents an update formula for the dead reckoningcovariance from a point A to a point B, in which a previous value of thematrix C_(temp) is used. It will be understood that the superscript “T”for the matrix G^(T) indicates the transpose of the matrix G. InEquation 11, d_(s) corresponds to the standard deviation of translationerror per unit of traveled distance, and r_(s) corresponds to thestandard deviation of rotation error per unit of traveled distance. Inone embodiment, d_(s) and r_(s) correspond to parameters with values of0.05 and 0.0001, respectively. Other appropriate values will be readilydetermined by one of ordinary skill in the art. In addition, it will beunderstood by the skilled practitioner that the mathematical formulaeexpressed in Equation 6 to Equation 11 can be expressed in manyalternative expressions such as by combining equations and changingvariable names. The process then increments the value for the loopvariable and returns to Equation 6. When the processing of the columnsof the data sub-matrix D is complete, the final value of a matrixC_(temp) is used as the odometer covariance C_(odom)(D) and applied toone or more particles.

In the loop that includes the beginning of the loop 1312, a state 1316,and the end of the loop 1320, the process associates a particle witheither a first group or a second group. A particle belonging to thefirst group is termed “primary particle” herein. A particle belonging tothe second group is termed a “dual particle” herein. The rate ofprobability at which particles are allocated to the first group istermed “primary rate” herein. It will be understood that the primaryrate can vary in a very broad range. An appropriate value for theprimary rate can depend on the relative reliability of dead reckoningmeasurements over relatively short distances. For example, where deadreckoning measurements are relatively unreliable, relatively lowervalues for the primary rate are used. In one embodiment, the primaryrate is about 0.9 such that about 90% of the particles are grouped asprimary particles and about 10% of the particles are grouped as dualparticles. In one embodiment, two different particles in the multiplehypotheses have about the same probability of being associated with thefirst group (e.g., a probability of 0.1) or with the second group (e.g.,a probability of 0.9), and such association is performed in anapproximately random manner. Upon completion of the loop, the processproceeds to the beginning of a loop 1324.

The beginning of the loop 1324 starts at a state 1326. In the state1326, the process predicts or estimates a new robot pose for the m-thparticle S_(m,pred) from the previous robot pose for the m-th particles_(m) (x_(m,old), y_(m,old), θ_(m,old))^(T) by retrieving the change inpose [Δ₁ ^(odom), Δ₂ ^(odom), Δ₃ ^(odom)]^(T) obtained via deadreckoning data as described earlier in connection with the state 1305 ofFIG. 13 and Equation 1, and combining the change in pose [Δ₁ ^(odom), Δ₂^(odom), Δ₃ ^(odom)]^(T) with the previous pose s_(m) (x_(m,old),y_(m,old), θ_(m,old))^(T) using the process described in the state 1110of FIG. 11 and in Equation 2, where (x_(k), y_(k), θ_(k))=(x_(m,old),y_(m,old), θ_(m,old)), and the new robot pose S_(m,pred) corresponds to(x_(l), y_(l), θ_(l))^(T)=(x_(m,pred), y_(m,pred), θ_(m,pred))^(T). Theprocess advances from the state 1326 to a decision block 1328.

In the decision block 1328, the process selects a primary processingpath 1332 or a dual processing path 1336. The primary processing path1332 includes a state 1344, a state 1348, and a state 1352. The dualprocessing path 1336 includes a state 1360, a state 1364, and a state1372.

In the state 1344, the process retrieves the predicted pose calculatedin the state 1326 and perturbs the predicted pose. In one embodiment,the process adds simulated Gaussian random noise (ε_(x,m), ε_(y,m),ε_(θ,m))^(T) to the predicted pose S_(m,pred)=(x_(m,pred), y_(m,pred),ε_(m,pred))^(T)=(S_(m,pred)(1), S_(m,pred)(2), S_(m,pred)(3))^(T) toproduce the perturbed predicted pose {tilde over (S)}_(m,pred). In oneembodiment, the simulated Gaussian random noise (ε_(x,m), ε_(y,m),ε_(θ,m))^(T) is sampled from a Gaussian distribution with mean (0, 0,0)^(T) and covariance C_(odom).

$\begin{matrix}{{\overset{\sim}{S}}_{m,{pred}} = \begin{pmatrix}{{S_{m,{pred}}(1)} + ɛ_{x,m}} \\{{S_{m,{pred}}(2)} + ɛ_{y,m}} \\{\left\lbrack {\left( {{S_{m,{pred}}(3)} + ɛ_{\theta,m} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}\end{pmatrix}} & {{Equation}\mspace{14mu} 12}\end{matrix}$

Equation 12 expresses a calculation for the perturbed predicted pose.The process advances from the state 1344 to the state 1348.

In the state 1348, the process predicts or estimates the landmark poseL_(m,meas) ^(k) from the perturbed predicted pose {tilde over(S)}_(m,pred) and from the visual measurements obtained from the visuallocalization module and/or the Pre-Filter module. Later, this predictedor estimated landmark pose L_(m,meas) ^(k) will be compared to a currentlandmark pose estimate L_(m) ^(k) from the current map of the landmarkassociated with the particle. Equation 14 expresses one formula that canbe used to predict the predicted landmark pose L_(m,meas) ^(k). Theprocess advances from the state 1348 to the state 1352.

$\begin{matrix}{\Delta^{vis} = {\begin{bmatrix}\Delta_{1}^{vis} \\\Delta_{2}^{vis} \\\Delta_{3}^{vis}\end{bmatrix} = \begin{bmatrix}\sqrt{\left( {\Delta\; x} \right)^{2} + \left( {\Delta\; y} \right)^{2}} \\{\arctan\left( {\Delta\;{y/\Delta}\; x} \right)} \\{\Delta\;\theta}\end{bmatrix}}} & {{Equation}\mspace{14mu} 13} \\{L_{m,{meas}}^{k} = \begin{bmatrix}{{{\overset{\sim}{S}}_{m,{pred}}(1)} + {\Delta_{1}^{vis}{\cos\left( {{{\overset{\sim}{S}}_{m,{pred}}(3)} + \Delta_{2}^{vis}} \right)}}} \\{{{\overset{\sim}{S}}_{m,{pred}}(2)} + {\Delta_{1}^{vis}{\sin\left( {{{\overset{\sim}{S}}_{m,{pred}}(3)} + \Delta_{2}^{vis}} \right)}}} \\{\left\lbrack {\left( {{{\overset{\sim}{S}}_{m,{pred}}(3)} + \Delta_{3}^{vis} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}\end{bmatrix}} & {{Equation}\mspace{14mu} 14}\end{matrix}$

In the state 1352, the process computes an importance factor w_(m) forthe primary processing path 1332. The importance factor w_(m) can becomputed by application of Equation 15 to Equation 18.

$\begin{matrix}{C = {C_{sensor} + C_{m}^{k}}} & {{Equation}\mspace{14mu} 15} \\{\Delta = {\begin{pmatrix}\Delta_{1} \\\Delta_{2} \\\Delta_{3}\end{pmatrix} = {L_{m,{meas}}^{k} - L_{m}^{k}}}} & {{Equation}\mspace{14mu} 16} \\{\Delta_{3} = {\left\lbrack {\left( {\Delta_{3} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}} & {{Equation}\mspace{14mu} 17} \\{w_{m} = {\frac{1}{2\pi\sqrt{\det\; C}}{\exp\left( {{- \frac{1}{2}}\Delta^{T}C^{- 1}\Delta} \right)}}} & {{Equation}\mspace{14mu} 18}\end{matrix}$

In Equation 15, the sensor covariance matrix C_(sensor) corresponds to a3×3 matrix. In one embodiment, the sensor covariance matrix C_(sensor)corresponds to a diagonal matrix diag(81 cm², 81 cm², 0,019 rad²). Otherrepresentative values for the elements of the covariance matrix will bereadily determined by one of ordinary skill in the art. For example,these appropriate values for the elements of the sensor covariancematrix C_(sensor) can vary with camera resolution, average distance to3-D features, and the like. In Equation 18, the “exp” indicatesexponentiation with a base of “e”; i.e., the base of natural logarithms.In one embodiment, the importance factor w_(m) is identifiably storedfor the particle and is later used to estimate the probability for theparticle. The process proceeds from the state 1352 to the end of theloop 1356. The process repeats the loop by returning from the end of theloop 1356 to the beginning of the loop 1324 to process other particles.Otherwise, the process proceeds to a state 1378.

Returning now to the decision block 1328, processing of particles thatare “dual” will now be described. The dual processing path 1336 startsat the state 1360.

In the state 1360, a hypothetical robot pose based on the current (lastupdated) pose estimate of the landmark, denoted L_(m) ^(k), and theacquired measurement of the landmark from the visual localization moduleand/or the Pre-Filter module, denoted Δx, Δy, and Δθ is computed. Thisproduces a visually-estimated robot pose termed a “measured pose”S_(m,meas), as illustrated in Equation 19.

$\begin{matrix}{S_{m,{meas}} = \begin{bmatrix}{{L_{m}^{k}(1)} - {\Delta_{1}^{vis}{\cos\left( {{L_{m}^{k}(3)} - \Delta_{2}^{vis}} \right)}}} \\{{L_{m}^{k}(2)} - {\Delta_{1}^{vis}{\sin\left( {{L_{m}^{k}(3)} - \Delta_{2}^{vis}} \right)}}} \\{\left\lbrack {\left( {{L_{m}^{k}(3)} - \Delta_{3}^{vis} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}\end{bmatrix}} & {{Equation}\mspace{14mu} 19}\end{matrix}$

In the state 1364, the measured pose S_(m,meas) is perturbed with randomnoise (ε_(x,m), ε_(y,m), ε_(θ,m))^(T) to generate a perturbed measuredpose {tilde over (S)}_(m,meas). In one embodiment, the random noise(ε_(x,m), ε_(y,m), ε_(θ,m))^(T) is sampled from a Gaussian distributionwith mean (0, 0, 0)^(T) and sensor covariance C_(sensor). Equation 20expresses a formula which can be used to calculate the perturbedmeasured pose {tilde over (S)}_(m,meas).

$\begin{matrix}{{\overset{\sim}{S}}_{m,{meas}} = \begin{pmatrix}{{S_{m,{meas}}(1)} + ɛ_{x,m}} \\{{S_{m,{meas}}(2)} + ɛ_{y,m}} \\{\left\lbrack {\left( {{S_{m,{meas}}(3)} + ɛ_{\theta,m} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}\end{pmatrix}} & {{Equation}\mspace{14mu} 20}\end{matrix}$

The process advances from the state 1364 to the state 1372. In the state1372, the process computes an importance factor w_(m) for the dualprocessing path 1336 by computing Equation 21, Equation 22, Equation 23,and Equation 24. Equation 24 corresponds to an expression for thecomputation of the importance factor w_(m) for a dual particle. A scalefactor of 1/f, such as ⅕, is applied in the computation of Equation 24to account for real-life visual measurements, which may exhibit actualnoise characteristics that differ from than that of Gaussian noise.

$\begin{matrix}{C = {C_{k}^{m} + C_{odom}}} & {{Equation}\mspace{14mu} 21} \\{\Delta = {\begin{pmatrix}\Delta_{1} \\\Delta_{2} \\\Delta_{3}\end{pmatrix} = {{\overset{\sim}{S}}_{m,{meas}} - S_{m,{pred}}}}} & {{Equation}\mspace{14mu} 22} \\{\Delta_{3} = {\left\lbrack {\left( {\Delta_{3} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}} & {{Equation}\mspace{14mu} 23} \\{w_{m} = {\left( \frac{1}{f} \right)\frac{1}{2\pi\sqrt{\det C}}{\exp\left( {{- \frac{1}{2}}\Delta^{T}C^{- 1}\Delta} \right)}}} & {{Equation}\mspace{14mu} 24}\end{matrix}$

The process then advances from the state 1372 to the end of the loop1356. The loop repeats until the processing of the particles iscomplete. When processing of the particles is complete, the processadvances to the state 1378. In the illustrated process, the state 1378,a state 1382, a state 1386, and a state 1390 correspond to a sub-processfor resampling particles in a probabilistic manner. Table 1 and Table 2illustrate an example of resampling particles. In the illustratedsub-process, particles that have a relatively high importance factor aremore likely to be retained than particles that have a relatively lowimportance factor.

TABLE 1 “Old” Importance Particle Number Factor (w) Normalized w Range(i) (ii) (iii) (iv) 1 0.99 0.430    0-0.430 2 0.29 0.126 0.430-0.556 30.48 0.209 0.556-0.765 4 0.31 0.135 0.765-0.900 5 0.23 0.100 0.900-1   

In the state 1378, at this point in the process, the robot poses for theparticles have been updated. In the state 1378, the process normalizesthe importance factors (w) associated with the particles. For example,the importance factors (w) for K particles can be summed, and then eachof the importance factors can be divided by the sum to normalize theimportance factors. By way of example, Table 1 illustrates normalizingfor a 5-particle multiple hypothesis system. The “old” particle numbersare listed in column (i) of Table 1. Sample values for the importancefactor (w) for each particle are listed in column (ii). The sum of thesample values in column (ii) is 2.30, which is applied as a divisor tothe importance factor values in column (ii) to generate the normalizedimportance factor values illustrated in column (iii). The processadvances from the state 1378 to the state 1382.

In the state 1382, the process generates selection ranges for theparticles. Returning to the example of Table 1, the process allocates arange within 0 to 1 according to the value of the normalized importancefactor. An example of a selection range for a particle based on thenormalized importance factor values is illustrated in column (iv) ofTable 1. Accordingly, particles associated with relatively large valuesof normalized importance factors have a correspondingly relatively largeselection range. The process advances from the state 1382 to the state1386.

In the state 1386, the process generates random numbers. These randomnumbers are used to select which of the old particles are copied andwhich of the old particles are rejected. Column (v) in Table 2represents “new” particle numbers. Column (vi) illustrates randomlygenerated numbers in the range of 0 to 1. It will be understood that therange for the randomly generated numbers illustrated in Column (vi) ofTable 2 should correspond to the same range as the selection range forthe particles, an example of which was described earlier in connectionwith column (iv) of Table 1. The process advances from the state 1386 tothe state 1390.

TABLE 2 “New” Randomly “Old” Particle Number Generated Number ParticleCopied (v) (vi) (vii) 1 0.01 1 2 0.08 1 3 0.54 2 4 0.64 3 5 0.84 4

In the state 1390, the process uses the randomly selected numbers forthe “new” particles illustrated in column (vi) of Table 2 and theselection ranges for the “old” particles illustrated in column (iv) ofTable 1 to select which particles are to be retained from the “old”collection of particles. The result of the resampling is illustrated incolumn (vii) of Table 2. In the example illustrated by Table 1 and byTable 2, the “old” particle 1 is copied twice to the new collection,“old” particles 2, 3, and 4 are each copied once to the collection, and“old” particle 5 is dropped from the collection.

In one embodiment, when an existing particle is copied, the history ofthe particle is also copied. For example, the records of a firstparticle with a first Hypothesis ID can be copied to the records of asecond particle with a second Hypothesis ID. Examples of fields forthese records were described in greater detail earlier in connectionwith the Multiple Hypothesis Table 814 and the Map Table 816 of FIG. 8.

The robot pose for the m-th particle can updated by using the time thatthe landmark was re-observed as a timestamp for the “last observed time”or t_(new) and using the predicted particle pose S_(m,pred) as the newpose data. This can be expressed in Equation 25.s _(m) =[t _(new) ,S _(m,meas) ^(T)]^(T)  Equation 25

The process can proceed from the state 1390 to an optional state 1394.In the optional state 1394, the process combines the multiple particledata for applications in which a single pose estimate is desired. Itwill be understood that the pose from the particles can be combined inmany ways. In one embodiment, the location and heading are computedaccording to an average of the poses, as expressed in Equation 26,Equation 27, and Equation 28, where s_(m)=(t, x_(m), y_(m), θ_(m)).

$\begin{matrix}{\overset{\_}{x} = {\frac{1}{K}{\sum\limits_{m = 1}^{K}{s_{m}(2)}}}} & {{Equation}\mspace{14mu} 26} \\{\overset{\_}{y} = {\frac{1}{K}{\sum\limits_{m = 1}^{K}{s_{m}(3)}}}} & {{Equation}\mspace{14mu} 27} \\{\overset{\_}{\theta} = {\arctan\left( \frac{\Sigma_{m = 1}^{K}{\sin\left( {s_{m}(4)} \right)}}{\Sigma_{m = 1}^{K}{\cos\left( {s_{m}(4)} \right)}} \right)}} & {{Equation}\mspace{14mu} 28}\end{matrix}$

A composite pose S can be provided by the SLAM process and can beexpressed by S=(x,y,θ)^(T). This composite pose S can be provided as aninput to, for example, a robot behavior control program such as abehavioral program for vacuum cleaning. In alternative examples ofcomputing a composite pose S, the composite pose can be weighted usingprobabilities and/or importance factors, can be averaged by selectingdata only from high probability particles, and the like. The processadvances from the optional state 1394 to a state 1398.

At this point in the illustrated process, the robot poses for theparticles have been updated and processed. In the state 1398, theprocess updates the maps for the particles by updating the landmark poseestimate L_(m) ^(k) for the observed landmark of a particle with astatistical filter. It will be understood that the state 1398 caninclude a loop to update the maps of each of the particles.

In one embodiment, a Kalman filter is used to update the landmark poseestimate L_(m) ^(k). The Kalman filter is an example of an efficientimplementation of a least-squares filter. The use of a Kalman filteradvantageously permits updates to the map to be made in real timewithout repeated iterations between measurements. A landmark pose andthe relative visual localization data (Δ₁ ^(vis), Δ₂ ^(vis), Δ₃^(vis))^(T) are used to generate the measured landmark pose {circumflexover (L)}_(m) ^(k) in the Kalman filter as illustrated in Equation 29.

$\begin{matrix}{{\hat{L}}_{m}^{k} = \begin{bmatrix}{{s_{m}(2)} + {\Delta_{1}^{vis}{\cos\left( {{s_{m}(4)} + \Delta_{2}^{vis}} \right)}}} \\{{s_{m}(3)} + {\Delta_{1}^{vis}{\sin\left( {{s_{m}(4)} + \Delta_{2}^{vis}} \right)}}} \\{\left\lbrack {\left( {{s_{m}(4)} + \Delta_{3}^{vis} + \pi} \right){mod}\; 2\pi} \right\rbrack - \pi}\end{bmatrix}} & {{Equation}\mspace{14mu} 29}\end{matrix}$

The measured landmark pose {circumflex over (L)}_(m) ^(k) is used in theKalman filter to efficiently update or correct the landmark poseestimate L_(m) ^(k). The covariance matrix of the process noise isdenoted by Q, and the covariance matrix of the measurement noise isdenoted by R. In one embodiment, an appropriate value for the covariancematrix of the process noise Q is the constant matrix diag(2.8 cm², 2.8cm², 0.0001 rad²). Other appropriate values for the covariance matrix ofthe process noise Q will be readily determined by one of ordinary skillin the art. In one embodiment, an appropriate value for the covariancematrix of the measurement noise R is the constant matrix diag(81 cm², 81cm², 0.0019 rad²). Other appropriate values for the measurement noise Rwill be readily apparent to one of ordinary skill in the art. It shouldbe noted that an appropriate value for the measurement noise R can varydepending on factors such as the resolution of the visual sensor, theaverage distance to features, and the like. The covariance matrix of thestate vector estimate C_(m) ^(k) is also updated in the Kalman filter.The process then ends and can be repeated on an ongoing basis as newlandmarks are re-encountered.

Example of System Architecture for the Visual Front End 602

FIG. 14 illustrates one embodiment of a system architecture for theVisual Front End 602. The illustrated system architecture for the VisualFront End 602 can advantageously be implemented with dedicated hardware,with firmware, with software, and with various combinations of the same.For clarity, relatively minor details such as timestamps, calibrationdata, and selected database identifiers are not drawn in FIG. 14.

The illustrated Visual Front End 602 includes an image acquisitioncontrol module 1402. The image acquisition control module 1402 receivesimage data 1404 as an input. The image data 1404 can be provided by avisual sensor, such as a camera. The visual sensor can correspond to asingle-camera device or to a multiple-camera device. The images acquiredcan be used to locate the robot within a global reference frame or canbe used to add landmarks to the landmark database 606. Where a singlecamera is used, and the robot moves to obtain images with varyingperspectives for landmark creation, the image acquisition control module1402 can also receive raw pose data 1406 from a dead reckoning device,such as from odometry. For example, the image acquisition control module1402 can select images for landmark creation that are taken atappropriately-spaced baseline intervals. In one embodiment, the imageacquisition control module 1402 advantageously receives the image data1404 and/or the raw pose data 1406 via interaction with a hardwareabstraction layer (HAL). See commonly-owned U.S. patent applicationentitled “SENSOR AND ACTUATOR ABSTRACTION AND AGGREGATION IN A HARDWAREABSTRACTION LAYER FOR A ROBOT” filed on Nov. 27, 2002, with applicationSer. No. 10/307,199, the entirety of which is hereby incorporated byreference herein.

The image acquisition control module 1402 provides image data as aninput to feature recognition logic 1408. The feature recognition logic1408 can identify feature descriptors, such as SIFT feature vectors,that are present in the image. The feature descriptors identified areprovided as an input to a landmark recognition module 1410. The landmarkrecognition module 1410 uses the feature descriptors identified in theimage to identify landmarks with similar features that have beenpreviously identified and stored in a database, such as the landmarkdatabase 606. In one embodiment, the landmark recognition module 1410can identify one or more matching landmarks based on the number offeatures that are common between the image and the stored landmark, andthe landmark recognition module 1410 can provide a relative localizationmodule 1412 with a list of matching landmarks and corresponding 2-Dfeatures. When no matching landmark is provided by the landmarkrecognition module 1410, a landmark generation module 1414 can determinewhether to add a new landmark to the landmark database 606 based on thefeatures observed in the image. In one embodiment, the relativelocalization module 1412 and the landmark generation module 1414 operateindependently in parallel such that a landmark can be added to thedatabase while another landmark is matched.

In one embodiment, the relative localization module 1412 receives anindication of which features matched in the one or more matchinglandmarks, and an indication of the approximate 2-D location of thefeatures in the image. For example, the indication of the approximate2-D location can correspond to a pixel location. The relativelocalization module 1412 extracts 3-D position information and 2-D imagelocation information for the features of the matching landmark from thelandmark database 606, and can use projection to calculate the robot'spose for the image data 1404 relative to the robot's pose when thelandmark was created. The relative localization module 1412 provides therelative pose(s) to the one or more identified landmarks as an output ofthe Visual Front End 602. The relative pose(s) can be provided as aninput to, for example, the Pre-Filter module 622 or to the SLAM module604.

The landmark generation module 1414 receives groups of image data, anddetermines whether to add a landmark to the landmark database 606. Thelandmark generation module 1414 uses groups of image data rather than asingle image so that the 3-D coordinates of a feature can be determined.When enough features of sufficient reliability have been identified, thelandmark generation module 1414 can add the landmark to the landmarkdatabase 606. In the illustrated example, the coordinates of the 3-Dfeatures and, optionally, the 2-D coordinates of the corresponding 2-Dfeatures and their associated feature descriptors are stored in thelandmark database 606. An indication that a new landmark has beencreated can be provided as an output of the Visual Front End 602 andprovided as an input to the SLAM module 604. It will be understood thatdatabase record identifiers can also be passed between various modulesfor bookkeeping purposes.

Example of a System Architecture for the Pre-Filter Module 622

FIG. 15 illustrates an embodiment of the Pre-Filter module 622 forfiltering outliers. The illustrated system architecture for thePre-Filter module 622 can advantageously be implemented with dedicatedhardware, with firmware, with software, and with various combinations ofthe same.

The Pre-Filter module 622 receives one or more indications of matchinglandmarks as an input and filters the matching landmarks such that none,a portion, or all of the matching landmarks are provided as an output.By filtering the measurements of matching landmarks according topredefined criteria such that outliers are not relied upon by the SLAMmodule 604 for localization, the Pre-Filter module 622 advantageouslyenhances the robustness and accuracy of the VSLAM system 600.

The illustrated Pre-Filter module 622 includes an outlier detectormodule 1502 and a filter module 1504. In one embodiment, the data shownin FIG. 15 as provided to the Pre-Filter module 622 is accessed from ashared memory by the pre-filter module. The outlier detector module 1502evaluates one or more criteria for evaluating the reliability ortrustworthiness of an observed landmark measurement. For example, one ormore of the RMS projection error, the number of matching features, andthe slope can be compared to predetermined thresholds as describedearlier in connection with FIG. 12. One or more landmark tags oridentifiers can also be provided as an input to the outlier detectormodule 1502 such that the outlier detector module 1502 can identifywhich of the identified landmarks have passed the tests for reliability.An indication of the pass or fail status of one or more landmarkmeasurements is provided by the outlier detector module 1502 as an inputto the filter module 1504.

At least partly in response to the pass or fail status of the landmarkmeasurements, the filter module 1504 permits the passage of the datafrom the Visual Front End 602 to the SLAM module 604. In one embodiment,the filtering is implemented by setting status flags that are read bythe SLAM module 604 to determine whether or not to use a particularlandmark.

Various embodiments of the invention have been described above. Althoughthis invention has been described with reference to these specificembodiments, the descriptions are intended to be illustrative of theinvention and are not intended to be limiting. Various modifications andapplications may occur to those skilled in the art without departingfrom the true spirit and scope of the invention as defined in theappended claims.

What is claimed is:
 1. A method of localizing a mobile device in amultiple-particle autonomous localization and mapping system, the methodcomprising: performing autonomous localization and mapping with aplurality of particles, wherein a particle includes a device pose and amap, wherein the map includes one or more landmarks; and calculating anupdated device pose estimate for a particle, wherein the technique usedto calculate the updated device pose estimate is selected according tothe type associated with the particle, wherein calculating the updateddevice pose estimate further comprises calculating the updated devicepose for a primary type of particle based at least in part on data froma dead reckoning sensor, and calculating the updated device poseestimate for a dual type of particle based at least in part on data froma visual sensor.
 2. The method as defined in claim 1, whereincalculating the updated device pose estimate further comprisescalculating the updated device pose estimate for a primary particlebased at least in part on data from a first sensor, and calculating theupdated device pose estimate for a dual particle based at least in parton data from a second sensor, where the second sensor is different fromthe first sensor.
 3. The method as defined in claim 1, whereincalculating the updated device pose estimate further comprises: using aprior pose estimate and dead reckoning sensor data to compute a new posefor particles of primary type based on the change in pose estimated fromthe dead reckoning sensor data; receiving a visually-measured relativepose to the landmark, wherein the landmark has been matched using visualfeatures from a visual sensor coupled to the mobile device, wherein therelative pose corresponds to a visually-measured difference in posebetween a landmark pose and a pose corresponding to the visualobservation; and using the landmark pose estimate and thevisually-measured relative pose to compute the new pose estimate forparticles of dual type.
 4. The method as defined in claim 1, furthercomprising: using dead reckoning data to estimate a change in pose froma prior update to the particles; wherein calculating the device pose forthe particle further comprises using the change in pose estimated fromdead reckoning data and using the relative pose to calculate an updateddevice pose estimate.
 5. The method as defined in claim 1, wherein thetechnique used to calculate the updated device pose estimate for thefirst type of particle further comprises: using a prior pose estimate,the dead reckoning sensor data, and simulated random noise to compute anew device pose estimate termed perturbed predicted device pose; usingthe perturbed predicted device pose and a visually-measured pose of thedevice relative to the landmark to compute a hypothetical landmark pose;computing an importance factor for the particle, wherein the importancefactor is based at least in part on an uncertainty measure and acomparison between the calculated hypothetical landmark pose and a priorlandmark pose estimate; and using the perturbed predicted pose with theimportance factor as the updated device pose.
 6. The method as definedin claim 5, wherein the simulated random noise exhibits an uncertaintymeasure for the dead reckoning sensor data.
 7. The method as defined inclaim 1, wherein the technique used to calculate the updated device posefor the second type of particle termed dual particle further comprises:using a landmark pose estimate corresponding to the landmark and avisually measured relative pose to compute a hypothetical device pose;perturbing the hypothetical device pose with simulated random noise;computing an importance factor for the particle, wherein the importancefactor is based at least in part on an uncertainty measure and acomparison between the hypothetical device pose and a predicted devicepose from a prior pose estimate and the dead reckoning sensor data; andusing the hypothetical device pose with the importance factor as theupdated device pose.
 8. The method as defined in claim 7, wherein thesimulated random noise exhibits an uncertainty measure estimated for avisual sensor.
 9. The method as defined in claim 1, further comprisingresampling selected particles from the plurality of particles.